#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 // /////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //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); //END -- 函数定义 ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 变量定义 // --- 任务ID --- static u16 xtell_i2c_test_id; static u16 collect_data_id; static u16 ble_send_data_id; static u16 sensor_read_data_id; static u16 calculate_data_id; // --- 环形缓冲区 --- #define SENSOR_DATA_BUFFER_SIZE 512 static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE]; static circle_buffer_t sensor_cb; static int count = 0; //--- test --- // 全局变量 u16 gsensor_id=0; u16 test_id=0; //END -- 变量定义 ////////////////////////////////////////////////////////////////////////////////////////////////// #if 0 BLE_send_data_t BLE_send_data; void test(){ signed short acc_data_buf[3] = {0}; signed short gyr_data_buf[3] = {0}; signed short acc_gyro_input[6] = {0}; float Angle_output[3] = {0}; // xlog("============start\n"); SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf); BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf); u8 data[50]; data[0] = 0xBB; data[1] = 0xBE; data[2] = 0x01; data[3] = sizeof(BLE_send_data_t); //后续包的数据长度 // send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4); memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t)); static int count = 0; if(count >=10){ count = 0; char* division = "==========\n"; send_data_to_ble_client(division,strlen(division)); char log_buffer[100]; // 100个字符应该足够了 // 使用 snprintf 进行格式化 int num_chars_written = snprintf( log_buffer, // 目标缓冲区 sizeof(log_buffer), // 目标缓冲区的最大容量 "s %d, %dcm/s, %dcm\n", // 格式化字符串 BLE_send_data.skiing_state, // 第一个 %d 的参数 BLE_send_data.speed_cms, // 第二个 %d 的参数 BLE_send_data.distance_cm // 第三个 %d 的参数 ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); extern BLE_KS_send_data_t KS_data; memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "Acc:%d, %d, %d\n", KS_data.acc_KS[0],KS_data.acc_KS[1],KS_data.acc_KS[2] ); // cm/s^2 send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "Gyr_dps:%d, %d, %d\n", KS_data.gyr_KS_dps[0], KS_data.gyr_KS_dps[1], KS_data.gyr_KS_dps[2] ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "angle: %d, %d, %d\n", KS_data.angle_KS[0], KS_data.angle_KS[1], KS_data.angle_KS[2] ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); extern debug_t debug1; extern debug_t debug2; memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "debug:%.2f,%.2f,%.2f(%.2f),%.2f\n", debug1.acc_variance, debug1.gyr_variance, debug1.acc_magnitude, debug2.acc_magnitude, //滤波后的加速度 debug1.gyr_magnitude ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); xlog("Call interval\n"); } count++; memset(&BLE_send_data, 0, sizeof(BLE_send_data_t)); memset(&data, 0, 50); // xlog("end============\n"); } #endif #define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储100个sensor_data_t元素 static circle_buffer_t sensor_read; // 环形缓冲区管理结构体 typedef struct { signed short acc_data[3]; signed short gyr_data[3]; float angle[3]; //pitch roll yaw float quaternion_output[4]; //四元数数据 } sensor_data_t; static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据 static circle_buffer_t sensor_send; // 环形缓冲区管理结构体 static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble要发送的数据 static u8 mutex1 = 0; static u8 mutex2 = 0; static int count_test1 = 0; static int count_test2 = 0; /** * @brief //读取传感器的数据放进缓冲区 * */ void sensor_read_data(){ // 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; if(circle_buffer_is_full(&sensor_read)){ // xlog("sensor_read_data: read buffer full\n"); return; } static sensor_data_t tmp; SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); // xlog("=======sensor_read_data middle 1\n"); memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short)); memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short)); if (!calibration_done) { //第1次启动，开启零漂检测 // status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); if(count > 100){ count = 0; char log_buffer[100]; // 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); } count++; if (status == 1) { calibration_done = 1; printf("Sensor calibration successful! Skiing mode is active.\n"); } } else { // printf("Calculate the time interval =============== start\n"); // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); if(mutex1 == 0){ mutex1 = 1; // count_test1++; // xlog("count_test_1: %d\n",count_test1); circle_buffer_write(&sensor_read, &tmp); mutex1 = 0; } extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); } // xlog("=======sensor_read_data END\n"); } void calculate_data(){ // xlog("=======start\n"); sensor_data_t tmp; if(circle_buffer_is_empty(&sensor_read)){ // xlog("sensor_read_buffer: read buffer empty\n"); return; } if(mutex1 == 0){ mutex1 = 1; circle_buffer_read(&sensor_read, &tmp); mutex1 = 0; }else{ return; } BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle,tmp.quaternion_output); if(circle_buffer_is_full(&sensor_send)) return; if(mutex2 == 0){ mutex2 = 1; circle_buffer_write(&sensor_send, &data_by_calculate); mutex2 = 0; } // extern void BLE_send_data(); // BLE_send_data(); // xlog("=======end\n"); } extern char xt_Check_Flag; void BLE_send_data(){ // xlog("=======start\n"); if(circle_buffer_is_empty(&sensor_send)){ // xlog("sensor_send_buffer: send buffer empty\n"); return; } #ifdef XTELL_TEST // #if 0 BLE_send_data_t tmp; if(mutex2 == 0){ mutex2 = 1; circle_buffer_read(&sensor_send, &tmp); mutex2 = 0; }else{ return; } if(count >=100){ // extern debug_t debug2; // xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm); // xlog("Acc:%d, %d, %d\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]); // xlog("Gyr:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]); // xlog("debug2.acc_magnitude:%.2f\n", debug2.acc_magnitude); int num_chars_written; count = 0; char* division = "==========\n"; send_data_to_ble_client(division,strlen(division)); char log_buffer[100]; // 100个字符应该足够了 // extern char iic_read_len; // extern char iic_write_result; // num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d, %d, %d\n", xt_Check_Flag, iic_read_len, iic_write_result); // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); #if 1 // 使用 snprintf 进行格式化 num_chars_written = snprintf( log_buffer, // 目标缓冲区 sizeof(log_buffer), // 目标缓冲区的最大容量 "s %d, %dcm/s, %dcm\n", // 格式化字符串 tmp.skiing_state, // 第一个 %d 的参数 tmp.speed_cms, // 第二个 %d 的参数 tmp.distance_cm // 第三个 %d 的参数 ); send_data_to_ble_client(&log_buffer, strlen(log_buffer)); memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2] ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "Gyr:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2] ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "Angle:%.1f, %.1f, %1.f\n", tmp.angle_data[0],tmp.angle_data[1],tmp.angle_data[2] ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); #endif short acc_mo_cms = sqrtf(tmp.acc_data[0]*tmp.acc_data[0] + tmp.acc_data[1]*tmp.acc_data[1] + tmp.acc_data[2]*tmp.acc_data[2])-900; memset(&log_buffer, 0, 100); num_chars_written = snprintf( log_buffer, sizeof(log_buffer), "acc_cm/s^2:%d\n", acc_mo_cms ); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); // xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm); // xlog("Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]); // xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]); } count++; // xlog("=======end\n"); #else #endif } //iic测试调用的 #if 0 static u16 xt_iic_test_id; char log_buffer_1[100]; extern char sen_log_buffer_1[100]; extern char sen_log_buffer_2[100]; extern char sen_log_buffer_3[100]; extern char sen_log_buffer_4[100]; extern char sen_log_buffer_5[100]; extern char w_log_buffer_1[100]; extern char w_log_buffer_2[100]; extern char w_log_buffer_3[100]; extern char w_log_buffer_4[100]; extern char w_log_buffer_5[100]; void xt_iic_test(){ char log_buffer[100]; send_data_to_ble_client(&log_buffer_1,strlen(log_buffer_1)); extern char iic_read_len; extern char iic_write_result; int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d,%d,%d\n", xt_Check_Flag, iic_read_len, iic_write_result); extern void send_data_to_ble_client(const u8* data, u16 length); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); if(sen_log_buffer_1 != NULL) send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1)); if(sen_log_buffer_2 != NULL) send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2)); if(sen_log_buffer_3 != NULL) send_data_to_ble_client(&sen_log_buffer_3,strlen(sen_log_buffer_3)); if(sen_log_buffer_4 != NULL) send_data_to_ble_client(&sen_log_buffer_4,strlen(sen_log_buffer_4)); if(sen_log_buffer_5 != NULL) send_data_to_ble_client(&sen_log_buffer_5,strlen(sen_log_buffer_5)); if(w_log_buffer_1 != NULL) send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1)); if(w_log_buffer_2 != NULL) send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2)); if(w_log_buffer_3 != NULL) send_data_to_ble_client(&w_log_buffer_3,strlen(w_log_buffer_3)); if(w_log_buffer_4 != NULL) send_data_to_ble_client(&w_log_buffer_4,strlen(w_log_buffer_4)); if(w_log_buffer_5 != NULL) send_data_to_ble_client(&w_log_buffer_5,strlen(w_log_buffer_5)); // SL_SC7U22_Config(); } #endif #if 0 u16 xt_iic_test_id; char hw_iic_init_result; void xt_hw_iic_test(){ char log_buffer[100]; extern char iic_read_len; extern char iic_write_result; int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"init result:%d, SL_SC7U22_Check=0x%d,%d,%d\n",hw_iic_init_result, xt_Check_Flag, iic_read_len, iic_write_result); extern void send_data_to_ble_client(const u8* data, u16 length); send_data_to_ble_client(&log_buffer,strlen(log_buffer)); } #endif void sensor_measure(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; static sensor_data_t tmp; mmc5603nj_mag_data_t mag_data; SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); // os_time_dly(1); mmc5603nj_read_mag_data(&mag_data); // xlog("=======sensor_read_data middle 1\n"); memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short)); memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short)); if (!calibration_done) { //第1次启动，开启零漂检测 // status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); 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++; if (status == 1) { calibration_done = 1; printf("Sensor calibration successful! Skiing mode is active.\n"); } } else { // printf("Calculate the time interval =============== start\n"); // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output); extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); } // mmc5603nj_mag_data_t mag_data; // mmc5603nj_read_mag_data(&mag_data); // float temperature = mmc5603nj_get_temperature(); // count_test1++; // if(count_test1 >500){ // count_test1 =0; // xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z); // } // xlog("=======sensor_read_data END\n"); } void xtell_task_create(void){ // int ret = hw_iic_init(0); // xlog("hw_iic_init result:%d\n",ret); // //初始化传感器 // SL_SC7U22_Config(); #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 // os_time_dly(10); // delay_2ms(10); // if(bmp280_init() != 0){ // xlog("bmp280 init error\n"); // } // float temp, press; // bmp280_read_data(&temp, &press); // xlog("get temp: %d, get press: %d\n",temp, press); // MPU9250_Mag_Init(); //iic总线设备扫描 // extern void i2c_scanner_probe(void); // i2c_scanner_probe(); xlog("xtell_task_create\n"); // 初始化环形缓冲区 // circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE); ano_protocol_init(115200); circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t)); circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t)); // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10); // // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4); // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1); #if 0 hw_iic_init_result = ret; create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000); #endif }