332 lines
11 KiB
C
332 lines
11 KiB
C
#include "system/includes.h"
|
|
#include "btstack/btstack_task.h"
|
|
#include "app_config.h"
|
|
#include "app_action.h"
|
|
#include "asm/pwm_led.h"
|
|
#include "tone_player.h"
|
|
#include "ui_manage.h"
|
|
#include "gpio.h"
|
|
#include "app_main.h"
|
|
#include "asm/charge.h"
|
|
#include "update.h"
|
|
#include "app_power_manage.h"
|
|
#include "audio_config.h"
|
|
#include "app_charge.h"
|
|
#include "bt_profile_cfg.h"
|
|
#include "dev_manager/dev_manager.h"
|
|
#include "update_loader_download.h"
|
|
#include "./sensor/SC7U22.h"
|
|
#include "./buffer/circle_buffer.h"
|
|
#include "btstack/avctp_user.h"
|
|
#include "calculate/skiing_tracker.h"
|
|
#include "xtell.h"
|
|
#include "./ano/ano_protocol.h"
|
|
#include "./sensor/MMC56.h"
|
|
#include "./sensor/BMP280.h"
|
|
#include "./sensor/AK8963.h"
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//宏定义
|
|
#define ENABLE_XLOG 1
|
|
#ifdef xlog
|
|
#undef xlog
|
|
#endif
|
|
#if ENABLE_XLOG
|
|
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
|
#else
|
|
#define xlog(format, ...) ((void)0)
|
|
#endif
|
|
|
|
#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储XXX个sensor_data_t元素
|
|
|
|
//
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//START -- 函数定义
|
|
void send_data_to_ble_client(const u8* data, u16 length);
|
|
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
|
|
extern void close_process(u16* pid,char* name);
|
|
//END -- 函数定义
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//START -- 变量定义
|
|
|
|
|
|
typedef struct {
|
|
// -- 六轴 --
|
|
signed short SC7U22_data[6];
|
|
// -- 磁力计 --
|
|
uint8_t mmc5603nj_buffer[9];
|
|
// -- 速度 --
|
|
uint16_t speed_cms;
|
|
// -- 气压计 --
|
|
int adc_P;
|
|
int adc_T;
|
|
} BLE_send_data_t;
|
|
|
|
static int count = 0;
|
|
|
|
// --- 环形缓冲区 ---
|
|
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
|
|
BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];
|
|
|
|
//END -- 变量定义
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/**
|
|
* @brief 六轴静态校准
|
|
*
|
|
*/
|
|
void SC7U22_static_calibration(void){
|
|
signed short acc_data_buf[3];
|
|
signed short gyr_data_buf[3];
|
|
float angle[3];
|
|
float quaternion_output[3];
|
|
|
|
static signed short combined_raw_data[6];
|
|
static int calibration_done = 0;
|
|
char status = 0;
|
|
static first_set_flag = 0;
|
|
if(first_set_flag == 0){
|
|
first_set_flag = 1;
|
|
set_SC7U22_Error_Flag(0);
|
|
}
|
|
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
|
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
|
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
|
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);
|
|
|
|
if(status == 1){ //校准完成
|
|
extern u16 SC7U22_calibration_id;
|
|
extern u8 SC7U22_init;
|
|
first_set_flag = 0;
|
|
SC7U22_init = 0x11;
|
|
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
|
|
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x00};
|
|
send2_1[4] = SC7U22_init;
|
|
send_data_to_ble_client(&send2_1,5);
|
|
}
|
|
if(count > 100){
|
|
count = 0;
|
|
char log_buffer[100];
|
|
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
|
|
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
|
xlog("status:%d\n", status);
|
|
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
|
|
}
|
|
count++;
|
|
}
|
|
|
|
/**
|
|
* @brief 开始采集传感器数据和计算速度
|
|
*
|
|
*/
|
|
void start_collect_fuc(void){
|
|
// xlog("=======sensor_read_data START\n");
|
|
|
|
static signed short combined_raw_data[6];
|
|
static int initialized = 0;
|
|
static int calibration_done = 0;
|
|
char status = 0;
|
|
BLE_send_data_t BLE_send_data_tmp;
|
|
uint8_t mmc5603nj_buffer[9];
|
|
signed short acc_data_buf[3];
|
|
signed short gyr_data_buf[3];
|
|
float angle[3];
|
|
float quaternion_output[3];
|
|
|
|
// -- 读数据 --
|
|
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
|
mmc5603nj_read_origin_data(mmc5603nj_buffer);
|
|
bmp280_read_originanl_data(&BLE_send_data_tmp.adc_P, &BLE_send_data_tmp.adc_T);
|
|
|
|
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
|
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
|
|
|
// -- 四元数 --
|
|
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
|
|
|
|
// -- 速度计算 --
|
|
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
|
|
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
|
|
uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
|
|
|
|
// -- 数据包装进结构体 --
|
|
memcpy(&BLE_send_data_tmp.SC7U22_data[0], acc_data_buf, 3 * sizeof(signed short));
|
|
memcpy(&BLE_send_data_tmp.SC7U22_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
|
memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
|
|
BLE_send_data_tmp.speed_cms = speed;
|
|
|
|
// -- 放进缓冲区 --
|
|
if(circle_buffer_is_full(&BLE_send_buff) == 0){
|
|
circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
/**
|
|
* @brief ble数据发送函数
|
|
*
|
|
*/
|
|
void BLE_send_fuc(void){
|
|
BLE_send_data_t data_to_send;
|
|
|
|
if (circle_buffer_is_empty(&BLE_send_buff) == 0) {
|
|
circle_buffer_read(&BLE_send_buff, &data_to_send);
|
|
} else {
|
|
// 缓冲区为空，直接返回
|
|
return;
|
|
}
|
|
|
|
// --- 封装并发送六轴传感器数据 ---
|
|
{
|
|
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(12) = 16字节
|
|
const uint8_t IMU_PACKET_LEN = 16;
|
|
const uint8_t IMU_PAYLOAD_LEN = 13; // 类型(1) + 数据(12)
|
|
const uint8_t IMU_TYPE = 0x01;
|
|
|
|
uint8_t imu_packet[IMU_PACKET_LEN];
|
|
|
|
// 填充包头
|
|
imu_packet[0] = 0xBB;
|
|
imu_packet[1] = 0xBE;
|
|
imu_packet[2] = IMU_PAYLOAD_LEN;
|
|
imu_packet[3] = IMU_TYPE;
|
|
// 拷贝六轴数据
|
|
// memcpy(&imu_packet[4], data_to_send.SC7U22_data, sizeof(data_to_send.SC7U22_data));
|
|
for (int i = 0; i < 6; i++) {
|
|
// SC7U22_data[i] 是一个 signed short (2字节)
|
|
// 将其低字节放在前面
|
|
imu_packet[4 + i * 2] = (uint8_t)(data_to_send.SC7U22_data[i] & 0xFF);
|
|
// 将其高字节放在后面
|
|
imu_packet[4 + i * 2 + 1] = (uint8_t)((data_to_send.SC7U22_data[i] >> 8) & 0xFF);
|
|
}
|
|
// xlog("imu %d\n",data_to_send.SC7U22_data[0]);
|
|
// xlog("imu_packet: 0x%x 0x%x 0x%x\n",imu_packet[4],imu_packet[5],imu_packet[6]);
|
|
send_data_to_ble_client(&imu_packet, IMU_PACKET_LEN);
|
|
}
|
|
|
|
// --- 封装并发送磁力计数据 ---
|
|
{
|
|
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(9) = 13字节
|
|
const uint8_t MAG_PACKET_LEN = 13;
|
|
const uint8_t MAG_PAYLOAD_LEN = 10; // 类型(1) + 数据(9)
|
|
const uint8_t MAG_TYPE = 0x02;
|
|
|
|
uint8_t mag_packet[MAG_PACKET_LEN];
|
|
|
|
// 填充包头
|
|
mag_packet[0] = 0xBB;
|
|
mag_packet[1] = 0xBE;
|
|
mag_packet[2] = MAG_PAYLOAD_LEN;
|
|
mag_packet[3] = MAG_TYPE;
|
|
// 拷贝磁力计数据
|
|
// memcpy(&mag_packet[4], data_to_send.mmc5603nj_buffer, sizeof(data_to_send.mmc5603nj_buffer));
|
|
for (int i = 0; i < 9; i++) {
|
|
mag_packet[4 + i] = data_to_send.mmc5603nj_buffer[i];
|
|
}
|
|
// xlog("mag: 0x%x 0x%x 0x%x\n",mag_packet[4],mag_packet[5],mag_packet[6]);
|
|
|
|
send_data_to_ble_client(&mag_packet, MAG_PACKET_LEN);
|
|
}
|
|
|
|
// --- 封装并发送压力机计数据 ---
|
|
{
|
|
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(8) = 12字节
|
|
const uint8_t PT_PACKET_LEN = 12;
|
|
const uint8_t PT_PAYLOAD_LEN = 9; // 类型(1) + 数据(8)
|
|
const uint8_t PT_TYPE = 0x03;
|
|
|
|
uint8_t pt_packet[PT_PACKET_LEN];
|
|
|
|
// 填充包头
|
|
pt_packet[0] = 0xBB;
|
|
pt_packet[1] = 0xBE;
|
|
pt_packet[2] = PT_PAYLOAD_LEN;
|
|
pt_packet[3] = PT_TYPE;
|
|
|
|
// 打包压力数据 data_to_send.adc_P (占 pt_packet[4] 到 pt_packet[7])
|
|
pt_packet[4] = (uint8_t)(data_to_send.adc_P & 0xFF); // 最低字节 (LSB)
|
|
pt_packet[5] = (uint8_t)((data_to_send.adc_P >> 8) & 0xFF);
|
|
pt_packet[6] = (uint8_t)((data_to_send.adc_P >> 16) & 0xFF);
|
|
pt_packet[7] = (uint8_t)((data_to_send.adc_P >> 24) & 0xFF); // 最高字节 (MSB)
|
|
|
|
// 打包温度数据 data_to_send.adc_T (占 pt_packet[8] 到 pt_packet[11])
|
|
pt_packet[8] = (uint8_t)(data_to_send.adc_T & 0xFF); // 最低字节 (LSB)
|
|
pt_packet[9] = (uint8_t)((data_to_send.adc_T >> 8) & 0xFF);
|
|
pt_packet[10] = (uint8_t)((data_to_send.adc_T >> 16) & 0xFF);
|
|
pt_packet[11] = (uint8_t)((data_to_send.adc_T >> 24) & 0xFF); // 最高字节 (MSB)
|
|
|
|
send_data_to_ble_client(&pt_packet, PT_PACKET_LEN);
|
|
}
|
|
|
|
|
|
// --- 封装并发送速度数据 ---
|
|
{
|
|
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(2) = 6字节
|
|
const uint8_t SPEED_PACKET_LEN = 6;
|
|
const uint8_t SPEED_PAYLOAD_LEN = 3; // 类型(1) + 数据(2)
|
|
const uint8_t SPEED_TYPE = 0x04;
|
|
|
|
uint8_t speed_packet[SPEED_PACKET_LEN];
|
|
|
|
// 填充包头
|
|
speed_packet[0] = 0xBB;
|
|
speed_packet[1] = 0xBE;
|
|
// 填充长度
|
|
speed_packet[2] = SPEED_PAYLOAD_LEN;
|
|
// 填充类型
|
|
speed_packet[3] = SPEED_TYPE;
|
|
|
|
// 小端模式
|
|
speed_packet[4] = (uint8_t)(data_to_send.speed_cms & 0xFF); // 低字节
|
|
speed_packet[5] = (uint8_t)((data_to_send.speed_cms >> 8) & 0xFF); // 高字节
|
|
|
|
send_data_to_ble_client(&speed_packet, SPEED_PACKET_LEN);
|
|
}
|
|
}
|
|
|
|
static u8 bmp280_test_id = 0;
|
|
void xtell_task_create(void){
|
|
|
|
#if TCFG_GSENOR_USER_IIC_TYPE
|
|
int ret = hw_iic_init(0);
|
|
xlog("init iic result:%d\n", ret); //返回0成功
|
|
#else
|
|
int ret = soft_iic_init(0);
|
|
int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);
|
|
|
|
#endif
|
|
|
|
// MPU9250_Mag_Init();
|
|
//iic总线设备扫描
|
|
// extern void i2c_scanner_probe(void);
|
|
// i2c_scanner_probe();
|
|
|
|
xlog("xtell_task_create\n");
|
|
|
|
circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
|
|
|
|
bmp280_init();
|
|
extern void bmp280_test(void);
|
|
xlog("barometer start measeure\n");
|
|
// create_process(&bmp280_test_id,"bmp280_test",NULL, bmp280_test, 100);
|
|
float Temp = 0;
|
|
float Press = 0;
|
|
xlog("test_func\n");
|
|
bmp280_read_data(&Temp, &Press);
|
|
xlog("Temp:%.2f, Press:%.2f\n",Temp,Press);
|
|
}
|
|
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//test
|
|
//
|
|
|
|
void bmp280_test(void){
|
|
|
|
} |