git

.gitignore

@@ -0,0 +1,32 @@
+# 编译生成的目标文件
+*.o
+*.so
+*.d
+
+# 编译生成的最终产物
+*.elf
+*.bin
+*.fw
+*.ufw
+*.map
+*.lst
+*.bc
+
+# 编译产物目录
+# 看起来你所有的编译结果都在 objs/ 目录下，直接忽略整个目录更方便
+/objs/
+
+# 工具链和构建产物
+# 根据你的路径，这些文件也应该被忽略
+/cpu/br28/tools/app.bin
+/cpu/br28/tools/data_code.bin
+/cpu/br28/tools/download/
+/cpu/br28/tools/isd_config.ini
+/cpu/br28/tools/sdk.elf*
+/cpu/br28/tools/sdk.lst
+/cpu/br28/tools/sdk.map
+/cpu/br28/sdk_used_list.used
+
+# VSCode 用户个人设置
+# 团队协作时，每个人的配置可能不同，通常不建议提交
+.vscode/settings.json

.vscode/settings.json

@@ -1,31 +0,0 @@
-{
-    "files.associations": {
-        "board_config.h": "c",
-        "board_jl701n_demo_cfg.h": "c",
-        "colorful_lights.h": "c",
-        "board_jl701n_anc_cfg.h": "c",
-        "update_loader_download.h": "c",
-        "iic_soft.h": "c",
-        "circle_buffer.h": "c",
-        "default_event_handler.h": "c",
-        "ui_manage.h": "c",
-        "charge.h": "c",
-        "app_main.h": "c",
-        "app_config.h": "c",
-        "app_action.h": "c",
-        "includes.h": "c",
-        "key_event_deal.h": "c",
-        "app_umidigi_chargestore.h": "c",
-        "hci_lmp.h": "c",
-        "bluetooth.h": "c",
-        "SCU722.C": "cpp",
-        "math.h": "c",
-        "avctp_user.h": "c",
-        "string.h": "c",
-        "dev_manager.h": "c",
-        "bt_tws.h": "c",
-        "skiing_tracker.h": "c",
-        "xtell.h": "c",
-        "debug.h": "c"
-    }
-}

Makefile

@@ -249,6 +249,10 @@ INCLUDES := \
 	-Iapps/earphone/xtell_Sensor/sensor \
 	-Iapps/earphone/xtell_Sensor \
 	-Iapps/earphone/xtell_Sensor/calculate \
+	-Iapps/earphone/xtell_Sensor/ano  \
+	-Iapps/earphone/xtell_Sensor/sensor/ \
+	-Iapps/earphone/xtell_Sensor/sensor/ \
+	-Iapps/earphone/xtell_Sensor/sensor/ \
 	-I$(SYS_INC_DIR) \
 
 
@@ -620,6 +624,10 @@ c_SRC_FILES := \
 	apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
 	apps/earphone/xtell_Sensor/sensor/SC7U22.c \
 	apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
+	apps/earphone/xtell_Sensor/ano/ano_protocol.c  \
+	apps/earphone/xtell_Sensor/sensor/MMC56.c \
+	apps/earphone/xtell_Sensor/sensor/BMP280.c \
+	apps/earphone/xtell_Sensor/sensor/AK8963.c \
 
 
 # 需要编译的 .S 文件

apps/common/device/gSensor/gSensor_manage.c

@@ -212,6 +212,44 @@ void write_gsensor_data_handle(void)
     }
 }
 
+// 临时的设备扫描诊断函数
+void i2c_scanner_probe(u8* device_addr, u8* found_number)
+{
+    printf("Starting I2C bus scan...\n");
+    int devices_found = 0;
+
+    // I2C地址范围是 0x08 到 0x77
+    for (uint8_t addr_7bit = 0x00; addr_7bit < 0x7F; addr_7bit++)
+    {
+        // 构建8位的写地址
+        uint8_t write_addr_8bit = (addr_7bit << 1);
+
+        iic_start(gSensor_info->iic_hdl);
+
+        // 尝试发送写地址，并检查返回值
+        // iic_tx_byte 返回 1 表示收到了 ACK
+        if (iic_tx_byte(gSensor_info->iic_hdl, write_addr_8bit))
+        {
+            device_addr[devices_found] = addr_7bit;
+            printf("=====================================================================\n");
+            printf("I2C device found at 7-bit address: 0x%02X\n", addr_7bit);
+            printf("I2C device found at 8-bit address: 0x%02X\n", write_addr_8bit);
+            printf("=====================================================================\n");
+            devices_found++;
+        }
+
+        iic_stop(gSensor_info->iic_hdl);
+        delay(gSensor_info->iic_delay); // 短暂延时
+    }
+    *found_number = devices_found;
+
+    if (devices_found == 0) {
+        printf("Scan finished. No I2C devices found.\n");
+    } else {
+        printf("Scan finished. Found %d device(s).\n", devices_found);
+    }
+}
+
 char w_log_buffer_1[100];
 char w_log_buffer_2[100];
 char w_log_buffer_3[100];
@@ -222,7 +260,10 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
     // spin_lock(&sensor_iic);
     /* os_mutex_pend(&SENSOR_IIC_MUTEX,0); */
     u8 ret = 1;
+    // xlog("iic_start\n");
     iic_start(gSensor_info->iic_hdl);
+
+    // xlog("iic_tx_byte id\n");
     if (0 == iic_tx_byte(gSensor_info->iic_hdl, w_chip_id)) {
         ret = 0;
         xlog("\n gsen iic wr err 0\n");
@@ -230,8 +271,10 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
         goto __gcend;
     }
 
+    // xlog("iic delay\n");
     delay(gSensor_info->iic_delay);
 
+    // xlog("iic_tx_byte: address\n");
     if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
         ret = 0;
         xlog("\n gsen iic wr err 1\n");
@@ -241,6 +284,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
 
     delay(gSensor_info->iic_delay);
 
+    // xlog("iic_tx_byte: command\n");
     if (0 == iic_tx_byte(gSensor_info->iic_hdl, function_command)) {
         ret = 0;
         xlog("\n gsen iic wr err 2\n");
@@ -249,6 +293,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
     }
 
     strcpy(&w_log_buffer_4, "gsen iic wr sucess\n");
+    // xlog("\n gsen iic wr sucess\n");
 
 __gcend:
     iic_stop(gSensor_info->iic_hdl);
@@ -271,7 +316,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
 
     iic_start(gSensor_info->iic_hdl);
     if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) {
-        xlog("\n gsen iic rd err 0\n");
+        xlog("I2C NACK on writing ADDR: 0x%X\n", r_chip_id - 1);
         read_len = 0;
         strcpy(&sen_log_buffer_1, "gsen iic rd err 0\n");
         goto __gdend;
@@ -280,7 +325,8 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
 
     delay(gSensor_info->iic_delay);
     if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
-        xlog("\n gsen iic rd err 1\n");
+        xlog("I2C NACK on register ADDR: 0x%X\n", register_address);
+        // xlog("\n gsen iic rd err 1\n");
         read_len = 0;
         strcpy(&sen_log_buffer_2, "gsen iic rd err 1\n");
         goto __gdend;
@@ -304,6 +350,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
     *buf = iic_rx_byte(gSensor_info->iic_hdl, 0);
     read_len ++;
     strcpy(&sen_log_buffer_4, "gsen iic rd success\n");
+    // xlog("\n gsen iic rd success\n");
 
 __gdend:
 

apps/common/third_party_profile/jieli/JL_rcsp/rcsp_updata/rcsp_adv_user_update.c

@@ -530,6 +530,7 @@ void JL_rcsp_msg_deal(void *hdl, u8 event, u8 *msg)
 
     case MSG_JL_ENTER_UPDATE_MODE:
         rcsp_printf("MSG_JL_ENTER_UPDATE_MODE:%x %x\n", msg[0], msg[1]);
+        clk_set("sys",96*1000000L);
         bt_set_low_latency_mode(0);
         if (support_dual_bank_update_en && !tws_api_get_role()) {
             u8 status = 0;

apps/earphone/app_main.c

@@ -57,14 +57,14 @@ const struct task_info task_info_table[] = {
 #else
     {"btstack",             3,     0,   768,   256 },
 #endif
-    {"audio_dec",           5,     0,   800,   128 },
+    // {"audio_dec",           5,     0,   800,   128 },
-    {"aud_effect",          5,     1,   800,   128 },
+    // {"aud_effect",          5,     1,   800,   128 },
     /*
      *为了防止dac buf太大，通话一开始一直解码，
      *导致编码输入数据需要很大的缓存，这里提高编码的优先级
      */
-    {"audio_enc",           6,     0,   768,   128 },
+    // {"audio_enc",           6,     0,   768,   128 },
-    {"aec",					2,	   1,   768,   128 },
+    // {"aec",					2,	   1,   768,   128 },
 #if TCFG_AUDIO_HEARING_AID_ENABLE
     {"HearingAid",			6,	   0,   768,   128   },
 #endif/*TCFG_AUDIO_HEARING_AID_ENABLE*/
@@ -86,9 +86,9 @@ const struct task_info task_info_table[] = {
     {"tws_ota_msg",			2,	   0,   256,   128 },
     {"dw_update",		 	2,	   0,   256,   128 },
     {"rcsp_task",		    2,	   0,   640,   128 },
-    {"aud_capture",         4,     0,   512,   256 },
+    // {"aud_capture",         4,     0,   512,   256 },
-    {"data_export",         5,     0,   512,   256 },
+    // {"data_export",         5,     0,   512,   256 },
-    {"anc",                 3,     1,   512,   128 },
+    // {"anc",                 3,     1,   512,   128 },
 #endif
 
 #if TCFG_GX8002_NPU_ENABLE
@@ -102,9 +102,9 @@ const struct task_info task_info_table[] = {
 #if TCFG_KWS_VOICE_RECOGNITION_ENABLE
     {"kws",                 2,     0,   256,   64  },
 #endif /* #if TCFG_KWS_VOICE_RECOGNITION_ENABLE */
-    {"usb_msd",           	1,     0,   512,   128 },
+    // {"usb_msd",           	1,     0,   512,   128 },
 #if !TCFG_USB_MIC_CVP_ENABLE
-    {"usbmic_write",       	2,     0,   256,   128 },
+    // {"usbmic_write",       	2,     0,   256,   128 },
 #endif
 #if AI_APP_PROTOCOL
     {"app_proto",           2,     0,   768,   64  },
@@ -112,12 +112,12 @@ const struct task_info task_info_table[] = {
 #if (TCFG_SPI_LCD_ENABLE||TCFG_SIMPLE_LCD_ENABLE)
     {"ui",           	    2,     0,   768,   256 },
 #else
-    {"ui",                  3,     0,   384 - 64,  128  },
+    // {"ui",                  3,     0,   384 - 64,  128  },
 #endif
 #if (TCFG_DEV_MANAGER_ENABLE)
     {"dev_mg",           	3,     0,   512,   512 },
 #endif
-    {"audio_vad",           1,     1,   512,   128 },
+    // {"audio_vad",           1,     1,   512,   128 },
 #if TCFG_KEY_TONE_EN
     {"key_tone",            5,     0,   256,   32  },
 #endif
@@ -137,7 +137,7 @@ const struct task_info task_info_table[] = {
     {"icsd_src",            2,     1,   512,   128 },
 #endif /*TCFG_AUDIO_ANC_ACOUSTIC_DETECTOR_EN*/
     {"pmu_task",            6,      0,  256,   128  },
-    {"WindDetect",          2,      0,  256,   128  },
+    // {"WindDetect",          2,      0,  256,   128  },
     {0, 0},
 };
 

apps/earphone/board/br28/board_jl701n_demo.c

@@ -249,7 +249,7 @@ const struct vad_mic_platform_data vad_mic_data = {
         .mic_ldo2PAD_en = 1,
         .mic_bias_en = 0,
         .mic_bias_res = 0,
-        .mic_bias_inside = TCFG_AUDIO_MIC0_BIAS_EN,
+        // .mic_bias_inside = TCFG_AUDIO_MIC0_BIAS_EN,
         /* ADC偏置电阻配置*/
         .adc_rbs = 3,
         /* ADC输入电阻配置*/
@@ -526,7 +526,7 @@ const struct hw_iic_config hw_iic_cfg[] = {
         .baudrate = TCFG_HW_I2C0_CLK,      //IIC通讯波特率
         .hdrive = 0,                       //是否打开IO口强驱
         .io_filter = 1,                    //是否打开滤波器（去纹波）
-        .io_pu = 1,                        //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1
+        .io_pu = 0,                        //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1
     },
 };
 

apps/earphone/board/br28/board_jl701n_demo_cfg.h

@@ -22,9 +22,11 @@
 //                                 UART配置                                        //
 //*********************************************************************************//
 #define TCFG_UART0_ENABLE					ENABLE_THIS_MOUDLE                     //串口打印模块使能
+// #define TCFG_UART0_ENABLE					0                     //串口打印模块使能
 #define TCFG_UART0_RX_PORT					NO_CONFIG_PORT                         //串口接收脚配置（用于打印可以选择NO_CONFIG_PORT）
 #define TCFG_UART0_TX_PORT  				IO_PORT_DP                             //串口发送脚配置
-#define TCFG_UART0_BAUDRATE  				1000000                                //串口波特率配置
+// #define TCFG_UART0_BAUDRATE  				1000000                                //串口波特率配置
+#define TCFG_UART0_BAUDRATE  				115200                                //串口波特率配置
 
 //*********************************************************************************//
 //                                 IIC配置                                        //
@@ -45,7 +47,7 @@
 
  */
 #define TCFG_HW_I2C0_PORTS                  'B'
-#define TCFG_HW_I2C0_CLK                    4000000                                  //硬件IIC波特率:100k
+#define TCFG_HW_I2C0_CLK                    1000000                                  //硬件IIC波特率:100k
 
 //*********************************************************************************//
 //                                 硬件SPI 配置                                        //
@@ -85,7 +87,7 @@
 //*********************************************************************************//
 //                                 USB 配置                                        //
 //*********************************************************************************//
-#define TCFG_PC_ENABLE                      1//DISABLE_THIS_MOUDLE//PC模块使能
+#define TCFG_PC_ENABLE                      0//DISABLE_THIS_MOUDLE//PC模块使能
 #define TCFG_UDISK_ENABLE					0//ENABLE_THIS_MOUDLE//U盘模块使能
 #define TCFG_OTG_USB_DEV_EN                 BIT(0)//USB0 = BIT(0)  USB1 = BIT(1)
 
@@ -104,7 +106,7 @@
 
 #define MULT_KEY_ENABLE						1//DISABLE 		//是否使能组合按键消息, 使能后需要配置组合按键映射表
 
-#define TCFG_KEY_TONE_EN					ENABLE//DISABLE xtell 		// 按键提示音。
+#define TCFG_KEY_TONE_EN					DISABLE//DISABLE xtell 		// 按键提示音。
 
 //*********************************************************************************//
 //                                 iokey 配置                                      //
@@ -727,9 +729,9 @@ DAC硬件上的连接方式,可选的配置：
 //   充电舱/蓝牙测试盒/ANC测试三者为同级关系,开启任一功能都会初始化PP0通信接口     //
 //*********************************************************************************//
 #define TCFG_CHARGESTORE_ENABLE				DISABLE_THIS_MOUDLE         //是否支持智能充电舱
-#define TCFG_TEST_BOX_ENABLE			    ENABLE_THIS_MOUDLE          //是否支持蓝牙测试盒
+#define TCFG_TEST_BOX_ENABLE			    DISABLE_THIS_MOUDLE//ENABLE_THIS_MOUDLE          //是否支持蓝牙测试盒  //xtell
 #define TCFG_ANC_BOX_ENABLE			        CONFIG_ANC_ENABLE           //是否支持ANC测试盒
-#define TCFG_UMIDIGI_BOX_ENABLE				ENABLE_THIS_MOUDLE			//是否支持UMIDIGI充电舱
+#define TCFG_UMIDIGI_BOX_ENABLE				DISABLE_THIS_MOUDLE			//是否支持UMIDIGI充电舱  //xtell
 #if TCFG_UMIDIGI_BOX_ENABLE
 #define _20MS_BIT							20							//传输20ms/Bit时使用
 #define _40MS_BIT							40							//传输40ms/Bit时使用
@@ -805,7 +807,7 @@ DAC硬件上的连接方式,可选的配置：
 //                                  EQ配置                                         //
 //*********************************************************************************//
 //EQ配置，使用在线EQ时，EQ文件和EQ模式无效。有EQ文件时，使能TCFG_USE_EQ_FILE,默认不用EQ模式切换功能
-#define TCFG_EQ_ENABLE                            1     //支持EQ功能,EQ总使能
+#define TCFG_EQ_ENABLE                            0     //支持EQ功能,EQ总使能
 // #if TCFG_EQ_ENABLE
 #define TCFG_EQ_ONLINE_ENABLE                     0     //支持在线EQ调试,如果使用蓝牙串口调试，需要打开宏 APP_ONLINE_DEBUG，否则，默认使用uart调试(二选一)
 #define TCFG_BT_MUSIC_EQ_ENABLE                   1     //支持蓝牙音乐EQ
@@ -1008,7 +1010,7 @@ DAC硬件上的连接方式,可选的配置：
 //*********************************************************************************//
 #define TCFG_USER_TWS_ENABLE                      0   //tws功能使能  
 #define TCFG_USER_BLE_ENABLE                      1   //BLE功能使能
-#define TCFG_BT_SUPPORT_AAC                       1   //AAC格式支持
+#define TCFG_BT_SUPPORT_AAC                       0   //AAC格式支持
 #define TCFG_BT_SUPPORT_LDAC                      0   //LDAC格式支持
 
 #if TCFG_BT_SUPPORT_LDAC
@@ -1082,7 +1084,7 @@ DAC硬件上的连接方式,可选的配置：
 //                           编解码格式配置(CodecFormat)                           //
 //*********************************************************************************//
 /*解码格式使能*/
-#define TCFG_DEC_MP3_ENABLE                 ENABLE
+#define TCFG_DEC_MP3_ENABLE                 DISABLE
 #define TCFG_DEC_WTGV2_ENABLE				ENABLE
 #define TCFG_DEC_G729_ENABLE                DISABLE
 #define TCFG_DEC_WMA_ENABLE					DISABLE

apps/earphone/board/br28/board_jl701n_demo_global_build_cfg.h

@@ -8,8 +8,8 @@
 
 /* Following Macros Affect Periods Of Both Code Compiling And Post-build */
 
-#define CONFIG_DOUBLE_BANK_ENABLE               0       //单双备份选择(若打开了改宏,FLASH结构变为双备份结构，适用于接入第三方协议的OTA， PS: JL-OTA同样支持双备份升级, 需要根据实际FLASH大小同时配置CONFIG_FLASH_SIZE)
+#define CONFIG_DOUBLE_BANK_ENABLE               1       //单双备份选择(若打开了改宏,FLASH结构变为双备份结构，适用于接入第三方协议的OTA， PS: JL-OTA同样支持双备份升级, 需要根据实际FLASH大小同时配置CONFIG_FLASH_SIZE)
-#define CONFIG_APP_OTA_ENABLE                   0       //是否支持RCSP升级(JL-OTA)
+#define CONFIG_APP_OTA_ENABLE                   1       //是否支持RCSP升级(JL-OTA)
 
 #define CONFIG_UPDATE_JUMP_TO_MASK              0   	//配置升级到loader的方式0为直接reset,1为跳转(适用于芯片电源由IO口KEEP住的方案,需要注意检查跳转前是否将使用DMA的硬件模块全部关闭)
 
@@ -33,7 +33,7 @@
 /* Following Macros Only For Post Bulid Configuaration */
 
 #define CONFIG_DB_UPDATE_DATA_GENERATE_EN       0       //是否生成db_data.bin(用于第三方协议接入使用)
-#define CONFIG_ONLY_GRENERATE_ALIGN_4K_CODE     0    	//ufw只生成1份4K对齐的代码
+#define CONFIG_ONLY_GRENERATE_ALIGN_4K_CODE     1    	//ufw只生成1份4K对齐的代码
 
 //config for supported chip version
 #ifdef CONFIG_BR30_C_VERSION

apps/earphone/include/app_config.h

@@ -65,8 +65,8 @@
 #if CONFIG_APP_OTA_ENABLE
 #define RCSP_UPDATE_EN		         1	   //是否支持rcsp升级
 #if CONFIG_DOUBLE_BANK_ENABLE              //双备份才能打开同步升级流程
-#define OTA_TWS_SAME_TIME_ENABLE     1     //是否支持TWS同步升级
+#define OTA_TWS_SAME_TIME_ENABLE     0     //是否支持TWS同步升级
-#define OTA_TWS_SAME_TIME_NEW        1     //使用新的tws ota流程
+#define OTA_TWS_SAME_TIME_NEW        0     //使用新的tws ota流程
 #else
 #define OTA_TWS_SAME_TIME_ENABLE     1//0  xtellota   //是否支持TWS同步升级
 #define OTA_TWS_SAME_TIME_NEW        1//0     //使用新的tws ota流程

apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c

@@ -0,0 +1,377 @@
+/*
+    使用四元数求角度和去掉重力分量
+*/
+#include "skiing_tracker.h"
+#include "../sensor/SC7U22.h"
+#include <math.h>
+#include <string.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 G_ACCELERATION 9.81f
+#define DEG_TO_RAD (3.14159265f / 180.0f)
+
+BLE_KS_send_data_t KS_data;
+static float quaternion_data[4];
+
+#ifdef XTELL_TEST
+
+debug_t debug1;
+debug_t debug2;
+#endif
+
+static skiing_tracker_t my_skiing_tracker;
+//////////////////////////////////////////////////////////////////////////////////////////////////
+//实现
+
+void clear_speed(void){
+    my_skiing_tracker.state = STATIC;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.speed = 0;
+}
+
+void start_detection(void){
+    my_skiing_tracker.state = STATIC;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.distance = 0;
+    my_skiing_tracker.speed = 0;
+}
+
+void stop_detection(void){
+    my_skiing_tracker.state = STOP_DETECTION;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.speed = 0;
+}
+
+/**
+ * @brief  初始化滑雪追踪器
+ * 
+ * @param tracker 
+ */
+void skiing_tracker_init(skiing_tracker_t *tracker)
+{
+    if (!tracker) {
+        return;
+    }
+    // 使用memset一次性清零整个结构体，包括新增的缓冲区
+    memset(tracker, 0, sizeof(skiing_tracker_t));
+    tracker->state = STATIC;
+}
+
+/**
+ * @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
+ */
+static void calculate_air_distance(skiing_tracker_t *tracker) {
+    float horizontal_speed_on_takeoff = sqrtf(
+        tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
+        tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
+    );
+    float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
+    tracker->distance += distance_in_air;
+}
+
+
+ /**
+  * @brief 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
+  * 
+  * @param acc_device 
+  * @param angle 
+  * @param acc_linear_world 
+  */
+static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_linear_world)
+{
+    // 1. 将输入的角度从度转换为弧度
+    // angle[0] -> pitch, angle[1] -> roll, angle[2] -> yaw
+    float pitch_rad = -angle[0] * (M_PI / 180.0f);
+    float roll_rad  = -angle[1] * (M_PI / 180.0f);
+    float yaw_rad   = -angle[2] * (M_PI / 180.0f);
+
+    // 2. 预先计算三角函数值，以提高效率
+    float c_r = cosf(roll_rad);
+    float s_r = sinf(roll_rad);
+    float c_p = cosf(pitch_rad);
+    float s_p = sinf(pitch_rad);
+    float c_y = cosf(yaw_rad);
+    float s_y = sinf(yaw_rad);
+
+    // 3. 构建从设备坐标系到世界坐标系的旋转矩阵 R_device_to_world
+    //    该矩阵基于 Z-Y-X (Yaw-Pitch-Roll) 欧拉角顺序
+    //    R = R_z(yaw) * R_y(pitch) * R_x(roll)
+    float R[3][3];
+    R[0][0] = c_y * c_p;
+    R[0][1] = c_y * s_p * s_r - s_y * c_r;
+    R[0][2] = c_y * s_p * c_r + s_y * s_r;
+
+    R[1][0] = s_y * c_p;
+    R[1][1] = s_y * s_p * s_r + c_y * c_r;
+    R[1][2] = s_y * s_p * c_r - c_y * s_r;
+
+    R[2][0] = -s_p;
+    R[2][1] = c_p * s_r;
+    R[2][2] = c_p * c_r;
+
+    // 4. 将设备坐标系的加速度计总读数旋转到世界坐标系
+    //    a_raw_world = R * acc_device
+    float ax_raw_world = R[0][0] * acc_device[0] + R[0][1] * acc_device[1] + R[0][2] * acc_device[2];
+    float ay_raw_world = R[1][0] * acc_device[0] + R[1][1] * acc_device[1] + R[1][2] * acc_device[2];
+    float az_raw_world = R[2][0] * acc_device[0] + R[2][1] * acc_device[1] + R[2][2] * acc_device[2];
+
+    // 5. 在世界坐标系中减去重力分量，得到线性加速度
+    //    假设世界坐标系Z轴垂直向上，重力矢量为 [0, 0, -g]
+    //    线性加速度 = 总加速度 - 重力加速度
+    //    az_linear = az_raw_world - (-g) = az_raw_world + g (如果Z轴向上)
+    //    az_linear = az_raw_world - (+g) = az_raw_world - g (如果Z轴向下)
+    //    这里我们采用 Z 轴向上的标准惯性系 (ENU)
+    
+    acc_linear_world[0] = ax_raw_world;
+    acc_linear_world[1] = ay_raw_world;
+    acc_linear_world[2] = az_raw_world - G_ACCELERATION; // Z轴向上，重力为正值，所以减去
+}
+/**
+ * @brief 在设备坐标系下，从原始加速度数据中移除重力分量
+ * @param acc_device        输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
+ * @param angle             输入：姿态角 [pitch, roll, yaw]，单位: 度
+ * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
+ */
+void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
+{
+    float pitch = -angle[0] * DEG_TO_RAD; // 绕 Y 轴
+    float roll  = -angle[1] * DEG_TO_RAD;  // 绕 X 轴
+    float yaw   = -angle[2] * DEG_TO_RAD;  // 绕 Z 轴
+
+    float cp = cosf(pitch);
+    float sp = sinf(pitch);
+    float cr = cosf(roll);
+    float sr = sinf(roll);
+    float cy = cosf(yaw);
+    float sy = sinf(yaw);
+
+    // 世界坐标系下的重力矢量
+    const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
+
+    // 计算旋转矩阵 R 的转置矩阵 R_transpose
+    // R (Z-Y-X) = 
+    // [ cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr]
+    // [ sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr]
+    // [ -sp,    cp*sr,             cp*cr            ]
+    //
+    // R_transpose = 
+    // [ cy*cp,              sy*cp,              -sp   ]
+    // [ cy*sp*sr - sy*cr,   sy*sp*sr + cy*cr,   cp*sr ]
+    // [ cy*sp*cr + sy*sr,   sy*sp*cr - cy*sr,   cp*cr ]
+    
+    // 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
+    // 由于 G_world 只有 z 分量，计算可以简化
+    float g_device[3];
+    g_device[0] = (-sp) * g_world[2];
+    g_device[1] = (cp * sr) * g_world[2];
+    g_device[2] = (cp * cr) * g_world[2];
+    
+    // 从原始设备加速度中减去重力投影
+    acc_linear_device[0] = acc_device[0] - g_device[0];
+    acc_linear_device[1] = acc_device[1] - g_device[1];
+    acc_linear_device[2] = acc_device[2] - g_device[2];
+}
+
+
+/**
+ * @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
+ * @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
+ * @param acc_device        输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
+ * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
+ */
+void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
+{
+    // 从四元数计算重力在设备坐标系下的投影
+    // G_device = R_transpose * G_world
+    // G_world = [0, 0, g]
+    // R_transpose 的第三列即为重力投影方向
+    float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
+    float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
+    float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+
+    // 从原始加速度中减去重力分量
+    acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
+    acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
+    acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
+}
+
+/**
+ * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
+ * @details 同样，此方法比使用欧拉角更优。
+ * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
+ * @param acc_linear_world  输出：世界坐标系下的线性加速度 [x, y, z]
+ */
+void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
+{
+    // 这是 R_device_to_world * acc_linear_device 的展开形式
+    acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
+                          (2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
+
+    acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
+
+    acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
+                          (2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
+    acc_linear_world[2] -= G_ACCELERATION;
+}
+
+
+/**
+ * @brief 主更新函数
+ * 
+ * @param tracker 
+ * @param acc_g 三轴加速度，g
+ * @param gyr_dps 三轴陀螺仪，dps
+ * @param angle 欧若拉角
+ * @param dt 采样时间间隔，会用来积分求速度
+ */
+void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
+{
+    if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
+        return;
+    }
+    if(my_skiing_tracker.state == STOP_DETECTION)
+        return;
+
+    // --- 数据预处理和缓冲 ---
+    float acc_device_ms2[3];
+    acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
+    acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
+    acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
+
+    #if 1  //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
+    float tmp_device_acc[3];
+    float tmp_world_acc[3];
+    // remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
+    // transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
+
+    q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
+    q_transform_to_world_with_quaternion(acc_device_ms2,quaternion_data,tmp_world_acc);
+
+    // 计算处理后加速度的水平模长
+    float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
+                                        tmp_device_acc[1] * tmp_device_acc[1] + 
+                                        tmp_device_acc[2] * tmp_device_acc[2]);
+
+    float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
+                                        tmp_world_acc[1] * tmp_world_acc[1] + 
+                                        tmp_world_acc[2] * tmp_world_acc[2]);
+    
+
+    static int count = 0;
+    if(count > 100){
+        xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
+        xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag);  //去掉重力加速度
+        xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag);  //去掉重力加速度
+        xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]);   //angle
+        xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
+        count = 0;
+        
+    }
+    count++;
+    
+    #endif
+
+}
+
+
+
+/**
+ * @brief 滑雪数据计算
+ * 
+ * @param acc_data_buf 传入的三轴加速度数据
+ * @param gyr_data_buf 传入的三轴陀螺仪数据
+ * @param angle_data   传入的欧若拉角数据
+ * @return BLE_send_data_t 要发送给蓝牙的数据
+ */
+BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
+    
+    static int initialized = 0;
+    static float acc_data_g[3];
+    static float gyr_data_dps[3];
+    if(quaternion != NULL){
+        memcpy(quaternion_data, quaternion,  4 * sizeof(float));
+    }
+
+    // const float delta_time = DELTA_TIME+0.01f;
+    // const float delta_time = DELTA_TIME + 0.005f;
+    const float delta_time = DELTA_TIME;
+    BLE_send_data_t BLE_send_data;
+    
+    if (!initialized) {
+        skiing_tracker_init(&my_skiing_tracker);
+        initialized = 1;
+        printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
+    }
+
+    
+    #if ACC_RANGE==2
+    // 加速度 LSB to g
+    acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
+    acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
+    acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
+    #endif
+
+    #if ACC_RANGE==4
+    // 加速度 LSB to g
+    acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
+    acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
+    acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
+    #endif
+
+    #if ACC_RANGE==8
+    //±8g	4096
+    acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
+    acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
+    acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
+	#endif
+
+    #if ACC_RANGE==16
+    //±16g	2048
+    acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
+    acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
+    acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
+	#endif
+
+    // 陀螺仪 LSB to dps (度/秒)
+    // ±2000dps量程下，转换系数约为 0.061
+    gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
+    gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
+    gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
+
+    skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
+
+    // BLE_send_data.skiing_state = my_skiing_tracker.state;
+    // for (int i = 0; i < 3; i++) {
+    //     #ifdef XTELL_TEST
+    //     BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
+    //     BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i];  //dps
+    //     BLE_send_data.angle_data[i] = angle_data[i];
+    //     #else
+    //     BLE_send_data.acc_data[i] = (short)acc_data_buf[i];   //原始adc数据
+    //     BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i];   //原始adc数据
+    //     BLE_send_data.angle_data[i] = angle_data[i];
+    //     #endif
+    // }
+    // BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
+    // BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
+    // // printf("Calculate the time interval =============== end\n");
+    
+    return BLE_send_data;
+}
+

apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h

@@ -0,0 +1,88 @@
+#ifndef SKIING_TRACKER_H
+#define SKIING_TRACKER_H
+
+#include "../xtell.h"
+// 定义滑雪者可能的状态
+typedef enum {
+    STATIC,                   // 静止或动态稳定：0
+    NO_CONSTANT_SPEED,        // 正在滑雪，非匀速：1
+    CONSTANT_SPEED,           // 正在滑雪，匀速：2
+    WOBBLE,                 // 正在原地旋转：3
+    WHEEL,                    // 转弯：4
+    FALLEN,                   // 已摔倒：5
+    TAKING_OFF,               // 起跳冲击阶段：6
+    IN_AIR,                   // 空中失重阶段：7
+    LANDING,                  // 落地冲击阶段：8
+    STOP_DETECTION,           // 停止检测：9
+    UNKNOWN                   // 未知状态：10
+} skiing_state_t;
+
+#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
+#define DELTA_TIME 0.01f
+
+
+// 追踪器数据结构体
+typedef struct {
+    // 公开数据
+    float velocity[3];          // 当前速度 (x, y, z)，单位: m/s
+    float distance;             // 总滑行距离，单位: m
+    float speed;                // 当前速率 (标量)，单位: m/s
+    skiing_state_t state;       // 当前滑雪状态
+
+    // 内部计算使用的私有成员
+    float acc_world[3];         // 在世界坐标系下的加速度
+
+    // 用于空中距离计算
+    float time_in_air;                     // 滞空时间计时器
+    float initial_velocity_on_takeoff[3];  // 起跳瞬间的速度向量
+    int airborne_entry_counter;      // 进入空中状态的确认计数器
+    int grounded_entry_counter;      // 落地确认计数器
+
+    // --- 内部计算使用的私有成员 ---
+    // 用于动态零速更新和旋转检测的缓冲区
+    float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
+    float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
+    int buffer_index;                          // 缓冲区当前索引
+    int buffer_filled;                         // 缓冲区是否已填满的标志
+
+    // 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
+    float acc_world_filtered[3];  //过滤过的
+    float acc_world_unfiltered_prev[3];  //上一次没过滤的
+
+    float acc_world_lpf[3];       // 经过低通滤波后的世界坐标系加速度
+} skiing_tracker_t;
+
+//ble发送的数据
+typedef struct{  //__attribute__((packed)){   //该结构体取消内存对齐
+    char sensor_state;
+    char skiing_state;
+    int speed_cms;        //求出的速度，cm/s
+    int distance_cm;      //求出的距离，cm
+    short acc_data[3];  //三轴加速度, g
+    short gyr_data[3];  //三轴陀螺仪, dps
+    float angle_data[3];     //欧若拉角
+}BLE_send_data_t;
+
+typedef struct{
+    int acc_KS[3];      //卡尔曼后，LSB转换后的  三轴加速度数据（cm/s^2）
+    int gyr_KS_dps[3];    //卡尔曼后，LSB to dps 三轴陀螺仪数据
+    int angle_KS[3];      //卡尔曼后，计算得到的欧若拉角数据
+}BLE_KS_send_data_t;
+
+#ifdef XTELL_TEST
+typedef struct{
+    float acc_variance;   //三轴加速度方差之和
+    float gyr_variance;   //三轴陀螺仪方差之和
+    float acc_magnitude;  //三轴加速度模长
+    float gyr_magnitude;  //三轴陀螺仪模长
+}debug_t;
+#endif
+/**
+ * @brief 初始化滑雪追踪器
+ *
+ * @param tracker 指向 skiing_tracker_t 结构体的指针
+ */
+void skiing_tracker_init(skiing_tracker_t *tracker);
+
+BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
+#endif // SKIING_TRACKER_H

apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c

@@ -0,0 +1,651 @@
+/*
+        
+*/
+#include "skiing_tracker.h"
+#include "../sensor/SC7U22.h"
+#include <math.h>
+#include <string.h>
+
+#define G_ACCELERATION 9.81f
+#define DEG_TO_RAD (3.14159265f / 180.0f)
+
+#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
+
+// --- 静止检测 ---
+//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
+// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
+#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
+// 陀螺仪方差阈值
+#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
+// 静止时候的陀螺仪模长
+#define STOP_GYR_MAG_THRESHOLD  15
+// --- --- --- 
+
+// --- 启动滑雪阈值 --- 
+// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
+#define START_ACC_MAG_THRESHOLD 1.0f  //0.5、1
+// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
+#define START_GYR_VARIANCE_THRESHOLD 15.0f
+// --- --- --- 
+
+// --- 滑雪过程 ---
+//加速度 模长(不含重力），低于此值视为 在做匀速运动
+#define SKIING_ACC_MAG_THRESHOLD 0.5f
+//陀螺仪 模长，高于此值视为 摔倒了
+#define FALLEN_GRY_MAG_THRESHOLD 2000.0f   //未确定
+// --- --- --- 
+
+// --- 原地旋转抖动 --- 
+// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
+#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
+// 加速度 模长 阈值
+#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
+// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
+#define ROTATION_GYR_MAG_THRESHOLD 30.0f  
+// --- --- --- 
+
+// --- 滑雪转弯动 --- 
+// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
+#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f  
+// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
+#define WHEEL_GYR_MAG_THRESHOLD 500.0f   // 
+// --- --- --- 
+
+// --- 跳跃 --- 
+// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
+#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
+// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
+#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
+// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
+#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
+// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
+#define AIRBORNE_CONFIRM_COUNT 3
+// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
+#define GROUNDED_CONFIRM_COUNT 2
+// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
+#define MAX_TIME_IN_AIR 12.5f 
+// --- --- --- 
+
+// --- 用于消除积分漂移的滤波器和阈值 ---
+// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
+// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
+#define HPF_ALPHA 0.999f  
+//0.995f： 0.08 Hz 的信号
+//0.999f： 0.0159 Hz 
+// --- --- --- 
+
+// --- 低通滤波器 ---
+// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
+// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
+#define LPF_ALPHA 0.7f
+
+// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
+// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
+//参考：0.2f ~ 0.4f
+#define ACC_DEAD_ZONE_THRESHOLD 0.05f   
+
+// --- 模拟摩擦力，进行速度衰减 ---
+#define SPEED_ATTENUATION 1.0f  //暂不模拟  
+BLE_KS_send_data_t KS_data;
+static float quaternion_data[4];
+#ifdef XTELL_TEST
+
+debug_t debug1;
+debug_t debug2;
+#endif
+
+static skiing_tracker_t my_skiing_tracker;
+//////////////////////////////////////////////////////////////////////////////////////////////////
+//实现
+
+void clear_speed(void){
+    my_skiing_tracker.state = STATIC;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.speed = 0;
+}
+
+void start_detection(void){
+    my_skiing_tracker.state = STATIC;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.distance = 0;
+    my_skiing_tracker.speed = 0;
+}
+
+void stop_detection(void){
+    my_skiing_tracker.state = STOP_DETECTION;
+    memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
+    my_skiing_tracker.speed = 0;
+}
+
+/**
+ * @brief  初始化滑雪追踪器
+ * 
+ * @param tracker 
+ */
+void skiing_tracker_init(skiing_tracker_t *tracker)
+{
+    if (!tracker) {
+        return;
+    }
+    // 使用memset一次性清零整个结构体，包括新增的缓冲区
+    memset(tracker, 0, sizeof(skiing_tracker_t));
+    tracker->state = STATIC;
+}
+
+/**
+ * @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
+ */
+static void calculate_air_distance(skiing_tracker_t *tracker) {
+    float horizontal_speed_on_takeoff = sqrtf(
+        tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
+        tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
+    );
+    float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
+    tracker->distance += distance_in_air;
+}
+
+
+
+/**
+ * @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
+ * @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
+ * @param acc_device        输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
+ * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
+ */
+void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
+{
+    // 从四元数计算重力在设备坐标系下的投影
+    // G_device = R_transpose * G_world
+    // G_world = [0, 0, g]
+    // R_transpose 的第三列即为重力投影方向
+    float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
+    float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
+    float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+
+    // 从原始加速度中减去重力分量
+    acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
+    acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
+    acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
+}
+
+/**
+ * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
+ * @details 同样，此方法比使用欧拉角更优。
+ * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
+ * @param acc_linear_world  输出：世界坐标系下的线性加速度 [x, y, z]
+ */
+void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
+{
+    // 这是 R_device_to_world * acc_linear_device 的展开形式
+    acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
+                          (2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
+
+    acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
+
+    acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
+                          (2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
+    // acc_linear_world[2] -= G_ACCELERATION;
+}
+
+
+
+/**
+ * @brief 计算缓冲区内三轴数据的方差之和
+ * 
+ * @param buffer 传进来的三轴数据：陀螺仪/加速度
+ * @return float 返回方差和
+ */
+static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
+{
+    float mean[3] = {0};
+    float variance[3] = {0};
+
+    // 计算均值
+    for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
+        mean[0] += buffer[i][0];
+        mean[1] += buffer[i][1];
+        mean[2] += buffer[i][2];
+    }
+    mean[0] /= VARIANCE_BUFFER_SIZE;
+    mean[1] /= VARIANCE_BUFFER_SIZE;
+    mean[2] /= VARIANCE_BUFFER_SIZE;
+
+    // 计算方差
+    for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
+        variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
+        variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
+        variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
+    }
+    variance[0] /= VARIANCE_BUFFER_SIZE;
+    variance[1] /= VARIANCE_BUFFER_SIZE;
+    variance[2] /= VARIANCE_BUFFER_SIZE;
+
+    // 返回三轴方差之和，作为一个综合的稳定度指标
+    return variance[0] + variance[1] + variance[2];
+}
+
+/**
+ * @brief 摩擦力模拟，进行速度衰减
+ * 
+ * @param tracker 
+ */
+void forece_of_friction(skiing_tracker_t *tracker){
+    // 增加速度衰减，模拟摩擦力
+    tracker->velocity[0] *= SPEED_ATTENUATION;
+    tracker->velocity[1] *= SPEED_ATTENUATION;
+    tracker->velocity[2] = 0; // 垂直速度强制归零
+}
+
+/**
+ * @brief 状态机更新
+ * 
+ * @param tracker 传入同步修改后传出
+ * @param acc_device_ms2 三轴加速度，m/s^2
+ * @param gyr_dps 三轴陀螺仪，dps
+ */
+static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
+{
+    // 缓冲区未填满时，不进行状态判断，默认为静止
+    if (!tracker->buffer_filled) {
+        tracker->state = STATIC;
+        return;
+    }
+
+    // --- 计算关键指标 ---
+    float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
+    float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
+    float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);  //dps
+    float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);  //m/s^s
+    float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
+
+    #ifdef XTELL_TEST
+    debug1.acc_variance =acc_variance;
+    debug1.gyr_variance =gyr_variance;
+    debug1.gyr_magnitude=gyr_magnitude;
+    debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
+    #endif
+
+
+
+   // --- 状态机逻辑 (核心修改区域) ---
+
+   #if 0  //暂时不考虑空中
+    // 1. 空中/落地状态的后续处理
+    if (tracker->state == IN_AIR) {
+        // A. 检测巨大冲击 -> 落地
+        if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
+            tracker->state = LANDING;
+        // B. 检测超时 -> 强制落地 (安全机制)
+        } else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
+            tracker->state = LANDING; 
+        // C. 检测恢复正常重力 (平缓落地)
+        } else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
+            tracker->grounded_entry_counter++;
+            if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
+                tracker->state = LANDING;
+            }
+        } else {
+            tracker->grounded_entry_counter = 0;
+        }
+        return; // 在空中或刚切换到落地，结束本次状态判断
+    }
+
+    // 2. 严格的 "起跳->空中" 状态转换逻辑
+    // 只有当处于滑行状态时，才去检测起跳意图
+    if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
+        if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
+            tracker->state = TAKING_OFF;
+            tracker->airborne_entry_counter = 0; // 准备检测失重
+            return;
+        }
+    }
+    // 只有在TAKING_OFF状态下，才去检测是否进入失重
+    if (tracker->state == TAKING_OFF) {
+        if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
+            tracker->airborne_entry_counter++;
+            if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
+                memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
+                tracker->time_in_air = 0;
+                tracker->state = IN_AIR;
+                tracker->airborne_entry_counter = 0;
+                tracker->grounded_entry_counter = 0;
+                return;
+            }
+        } else {
+            // 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
+            // 可以加一个小的超时计数器，这里为了简单先直接恢复
+            tracker->state = NO_CONSTANT_SPEED;
+        }
+        return; // 无论是否切换，都结束本次判断
+    }
+    #endif
+
+    
+    // --- 静止判断 ---
+    if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
+        tracker->state = STATIC;
+        return;
+    }
+
+    // --- 地面状态切换逻辑 ---
+    switch (tracker->state) {
+        case LANDING: 
+            tracker->state = STATIC;
+            break;
+        case STATIC:
+            // 优先判断是否进入 WOBBLE 状态
+            // 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
+            if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
+                tracker->state = WOBBLE;
+            } 
+            // 只有在陀螺仪和加速度都满足“前进”特征时，才启动
+            else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            break;
+
+        case WOBBLE:
+            // 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
+            if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            // 如果陀螺仪活动减弱，则可能恢复静止
+            else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
+                // 不直接跳回STATIC，而是依赖下一轮的全局静止判断
+            }
+            break;
+        case NO_CONSTANT_SPEED:  //非匀速状态
+            //暂时不考虑摔倒
+            // if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
+            //     tracker->state = FALLEN;  //摔倒
+            // } else 
+            if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
+                tracker->state = WHEEL;  //转弯
+            } else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
+                tracker->state = CONSTANT_SPEED;  //匀速
+            }
+            break;
+
+        case CONSTANT_SPEED:  //匀速状态
+            if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { 
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            //TODO：可以添加进入转弯或摔倒的判断
+            break;
+
+        case WHEEL:
+            // 从转弯状态，检查转弯是否结束
+            // 如果角速度和加速度方差都降下来了，就回到普通滑行状态
+            if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            break;
+
+        case FALLEN:
+            // TODO：回到 STATIC
+            break;
+    }
+
+
+}
+
+
+/**
+ * @brief 主更新函数
+ * 
+ * @param tracker 
+ * @param acc_g 三轴加速度，g
+ * @param gyr_dps 三轴陀螺仪，dps
+ * @param angle 欧若拉角
+ * @param dt 采样时间间隔，会用来积分求速度
+ */
+void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
+{
+    if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
+        return;
+    }
+    if(my_skiing_tracker.state == STOP_DETECTION)
+        return;
+
+    // --- 数据预处理和缓冲 ---
+    float acc_device_ms2[3];
+    acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
+    acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
+    acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
+
+    // 将最新数据存入缓冲区
+    memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
+    memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
+    
+    tracker->buffer_index++;
+    if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
+        tracker->buffer_index = 0;
+        tracker->buffer_filled = 1; // 标记缓冲区已满
+    }
+
+    // --- 更新状态机 ---
+    update_state_machine(tracker, acc_device_ms2, gyr_dps);
+
+    // --- 根据状态执行不同的计算逻辑 ---
+    switch (tracker->state) {
+        case TAKING_OFF:
+            tracker->speed = 0.0f;
+            break;
+        case IN_AIR:
+            // 在空中时，只累加滞空时间
+            tracker->time_in_air += dt;
+            break;
+        case LANDING:
+            // 刚落地，计算空中距离
+            calculate_air_distance(tracker);
+            // 清理速度和滤波器状态，为恢复地面追踪做准备
+            memset(tracker->velocity, 0, sizeof(tracker->velocity));
+            tracker->speed = 0;
+            memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
+            memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
+            memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
+            break;
+        case WHEEL:
+        case NO_CONSTANT_SPEED:
+            float linear_acc_device[3];
+            float linear_acc_world[3];
+            // 在设备坐标系下，移除重力，得到线性加速度
+            q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
+
+            // 将设备坐标系下的线性加速度，旋转到世界坐标系
+            q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
+             // 将最终用于积分的加速度存入 tracker 结构体
+            memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
+
+            float acc_world_temp[3]; // 临时变量存储当前周期的加速度
+            for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
+
+                // --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
+
+                // 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
+                //    HPF的瞬态响应会产生尖峰，这是正常的。
+                tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
+                tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
+
+                // 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
+                //    这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
+                tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
+                
+                // 将最终处理完的加速度值存入临时变量
+                acc_world_temp[i] = tracker->acc_world_lpf[i];
+            }
+
+            // 计算处理后加速度的水平模长
+            float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
+                                                acc_world_temp[1] * acc_world_temp[1]);
+            #if XTELL_TEST
+            debug2.acc_magnitude = acc_horizontal_mag;
+            #endif
+            // 应用死区，并积分
+            if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
+                tracker->velocity[0] += acc_world_temp[0] * dt;
+                tracker->velocity[1] += acc_world_temp[1] * dt;
+            }
+
+            // 更新速度和距离
+            tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
+                                tracker->velocity[1] * tracker->velocity[1]);
+            tracker->distance += tracker->speed * dt;
+            break;
+        case CONSTANT_SPEED:
+            //保持上次的速度不变。只更新距离
+            tracker->distance += tracker->speed * dt;
+            break;
+        case STATIC:
+        case WOBBLE:
+            // 速度清零，抑制漂移
+            memset(tracker->velocity, 0, sizeof(tracker->velocity));
+            tracker->speed = 0.0f;
+            memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
+            memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
+            memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
+            #if XTELL_TEST
+            debug2.acc_magnitude = 0;
+            #endif
+            break;   
+        case FALLEN:
+            // TODO
+            break;
+        default:
+            break;
+    }
+
+
+   
+
+    #if 1
+    float linear_acc_device[3];
+    float linear_acc_world[3];
+    float tmp_world_acc[3];
+    // 在设备坐标系下，移除重力，得到线性加速度
+    q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
+
+    // 将设备坐标系下的线性加速度，旋转到世界坐标系
+    q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
+
+    
+    float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
+                                        tmp_world_acc[1] * tmp_world_acc[1] + 
+                                        tmp_world_acc[2] * tmp_world_acc[2]);
+    
+    static int count = 0;
+    if(count > 100){
+        xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
+        xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag);  //去掉重力加速度
+        xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]);   //angle
+        xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
+        count = 0;
+        
+    }
+    count++;
+    #endif
+
+
+}
+
+
+
+/**
+ * @brief 滑雪数据计算
+ * 
+ * @param acc_data_buf 传入的三轴加速度数据
+ * @param gyr_data_buf 传入的三轴陀螺仪数据
+ * @param angle_data   传入的欧若拉角数据
+ * @return BLE_send_data_t 要发送给蓝牙的数据
+ */
+BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
+    
+    static int initialized = 0;
+    static float acc_data_g[3];
+    static float gyr_data_dps[3];
+
+    if(quaternion != NULL){
+        memcpy(quaternion_data, quaternion,  4 * sizeof(float));
+    }
+
+    // const float delta_time = DELTA_TIME+0.01f;
+    // const float delta_time = DELTA_TIME + 0.005f;
+    const float delta_time = DELTA_TIME;
+    BLE_send_data_t BLE_send_data;
+    
+    if (!initialized) {
+        skiing_tracker_init(&my_skiing_tracker);
+        initialized = 1;
+        printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
+    }
+
+    
+    #if ACC_RANGE==2
+    // 加速度 LSB to g
+    acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
+    acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
+    acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
+    #endif
+
+    #if ACC_RANGE==4
+    // 加速度 LSB to g
+    acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
+    acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
+    acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
+    #endif
+
+    #if ACC_RANGE==8
+    //±8g	4096
+    acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
+    acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
+    acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
+	#endif
+
+    #if ACC_RANGE==16
+    //±16g	2048
+    acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
+    acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
+    acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
+	#endif
+
+    // 陀螺仪 LSB to dps (度/秒)
+    // ±2000dps量程下，转换系数约为 0.061
+    gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
+    gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
+    gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
+
+    skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
+
+    BLE_send_data.skiing_state = my_skiing_tracker.state;
+    for (int i = 0; i < 3; i++) {
+        #ifdef XTELL_TEST
+        BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
+        BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i];  //dps
+        BLE_send_data.angle_data[i] = angle_data[i];
+        #else
+        BLE_send_data.acc_data[i] = (short)acc_data_buf[i];   //原始adc数据
+        BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i];   //原始adc数据
+        BLE_send_data.angle_data[i] = angle_data[i];
+        #endif
+    }
+    BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
+    BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
+    // printf("Calculate the time interval =============== end\n");
+    
+    return BLE_send_data;
+}
+

apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h

@@ -0,0 +1,88 @@
+#ifndef SKIING_TRACKER_H
+#define SKIING_TRACKER_H
+
+#include "../xtell.h"
+// 定义滑雪者可能的状态
+typedef enum {
+    STATIC,                   // 静止或动态稳定：0
+    NO_CONSTANT_SPEED,        // 正在滑雪，非匀速：1
+    CONSTANT_SPEED,           // 正在滑雪，匀速：2
+    WOBBLE,                 // 正在原地旋转：3
+    WHEEL,                    // 转弯：4
+    FALLEN,                   // 已摔倒：5
+    TAKING_OFF,               // 起跳冲击阶段：6
+    IN_AIR,                   // 空中失重阶段：7
+    LANDING,                  // 落地冲击阶段：8
+    STOP_DETECTION,           // 停止检测：9
+    UNKNOWN                   // 未知状态：10
+} skiing_state_t;
+
+#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
+#define DELTA_TIME 0.01f
+
+
+// 追踪器数据结构体
+typedef struct {
+    // 公开数据
+    float velocity[3];          // 当前速度 (x, y, z)，单位: m/s
+    float distance;             // 总滑行距离，单位: m
+    float speed;                // 当前速率 (标量)，单位: m/s
+    skiing_state_t state;       // 当前滑雪状态
+
+    // 内部计算使用的私有成员
+    float acc_no_g[3];         // 去掉重力分量后的加速度
+
+    // 用于空中距离计算
+    float time_in_air;                     // 滞空时间计时器
+    float initial_velocity_on_takeoff[3];  // 起跳瞬间的速度向量
+    int airborne_entry_counter;      // 进入空中状态的确认计数器
+    int grounded_entry_counter;      // 落地确认计数器
+
+    // --- 内部计算使用的私有成员 ---
+    // 用于动态零速更新和旋转检测的缓冲区
+    float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
+    float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
+    int buffer_index;                          // 缓冲区当前索引
+    int buffer_filled;                         // 缓冲区是否已填满的标志
+
+    // 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
+    float acc_world_filtered[3];  //过滤过的
+    float acc_world_unfiltered_prev[3];  //上一次没过滤的
+
+    float acc_world_lpf[3];       // 经过低通滤波后的世界坐标系加速度
+} skiing_tracker_t;
+
+//ble发送的数据
+typedef struct{  //__attribute__((packed)){   //该结构体取消内存对齐
+    char sensor_state;
+    char skiing_state;
+    int speed_cms;        //求出的速度，cm/s
+    int distance_cm;      //求出的距离，cm
+    short acc_data[3];  //三轴加速度, g
+    short gyr_data[3];  //三轴陀螺仪, dps
+    float angle_data[3];     //欧若拉角
+}BLE_send_data_t;
+
+typedef struct{
+    int acc_KS[3];      //卡尔曼后，LSB转换后的  三轴加速度数据（cm/s^2）
+    int gyr_KS_dps[3];    //卡尔曼后，LSB to dps 三轴陀螺仪数据
+    int angle_KS[3];      //卡尔曼后，计算得到的欧若拉角数据
+}BLE_KS_send_data_t;
+
+#ifdef XTELL_TEST
+typedef struct{
+    float acc_variance;   //三轴加速度方差之和
+    float gyr_variance;   //三轴陀螺仪方差之和
+    float acc_magnitude;  //三轴加速度模长
+    float gyr_magnitude;  //三轴陀螺仪模长
+}debug_t;
+#endif
+/**
+ * @brief 初始化滑雪追踪器
+ *
+ * @param tracker 指向 skiing_tracker_t 结构体的指针
+ */
+void skiing_tracker_init(skiing_tracker_t *tracker);
+
+BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
+#endif // SKIING_TRACKER_H

apps/earphone/xtell_Sensor/README.md

@@ -32,6 +32,7 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
 
 
 # 11.13
+
 代码主要文件夹：apps\earphone\xtell_Sensor
 
 - apps\earphone\xtell_Sensor\send_data.c，‘ xtell_task_create ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务
@@ -53,3 +54,42 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
   - 要使用只需要把代码复制粘贴到calculate文件夹下
 
 
+
+# 11.18
+
+去除重力分量后仍有误差：
+
+- 数据对齐？
+- 有没有丢数据？
+- 重复定位的数据？
+- 静态时的角度误差？
+
+
+
+定时器1的回调函数（10ms调用一次）**A**：读取传感器数据，放进buff
+
+定时器2的回调函数（5ms调用一次）**B**：读取buff的传感器数据，去除重力分离的计算
+
+- **数据没有对齐**，A 的回调调用计数 > B 的回调调用计数
+- **丢数据了**，A 读取传感器数据的回调函数中，打印了buff已满的log
+- **重复定位**：移动后回到原先的位置，前后的计算得到的三轴角度相同
+- **静态时的角度误差**：1°左右
+- 定时器2不进行重力分离计算，只进行计数，也仍然有数据没有对齐和丢数据的情况
+
+
+
+将读取传感器数据、去除重力分量计算放到同一个任务下，同步进行
+
+- 数据没有丢失，数据也对齐了 
+  - 在小倾斜的坡面下，去除重力分量后的总的加速度，**小于0.1m/s^2**
+  - 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.4m/s^2上下**
+- 貌似是角度越大，越接近方向锁，导致角度更容易漂移造错数据错误
+
+采用四元数的方式做去除重力分量的计算：
+
+- 将读取传感器数据、去除重力分量计算放到同一个任务下
+
+  - 在小倾斜的坡面下，去除重力分量后的总的加速度，低于**0.04m/s^2**
+  - 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.1m/s^2上下**
+
+  - 大倾斜角度的误差要靠磁力计来消除（yaw无法通过加速度计来消除偏差）

apps/earphone/xtell_Sensor/ano/ano_protocol.c

@@ -0,0 +1,166 @@
+/*
+    发送数据给上位机的,需要将log打印出口关闭
+*/
+
+#include "ano_protocol.h"
+#include "asm/uart_dev.h"
+#include "app_config.h" 
+
+// 定义协议常量
+#define ANO_FRAME_HEADER    0xAA
+#define ANO_TO_COMPUTER_ADDR 0xFF
+
+// 用于保存 uart_dev_open 返回的句柄
+static const uart_bus_t *ano_uart_dev = NULL;
+
+/**
+ * @brief 计算并填充协议的校验和
+ * @param frame_buffer 指向数据帧缓冲区的指针
+ */
+static void ano_calculate_checksum(u8 *frame_buffer) {
+    #if TCFG_UART0_ENABLE==0
+    u8 sum_check = 0;
+    u8 add_check = 0;
+    
+    // 数据长度在索引为 3 的位置
+    u8 data_len = frame_buffer[3];
+    // 需要计算校验和的总长度 = 帧头(1) + 地址(1) + ID(1) + 长度(1) + 数据(data_len)
+    u16 checksum_len = 4 + data_len;
+
+    for (u16 i = 0; i < checksum_len; i++) {
+        sum_check += frame_buffer[i];
+        add_check += sum_check;
+    }
+
+    // 将计算出的校验和填充到帧的末尾
+    frame_buffer[checksum_len] = sum_check;
+    frame_buffer[checksum_len + 1] = add_check;
+    #endif
+}
+
+/**
+ * @brief 初始化用于上位机通信的串口
+ */
+int ano_protocol_init(u32 baudrate) {
+    #if TCFG_UART0_ENABLE==0
+    // 防止重复初始化
+    if (ano_uart_dev) {
+        return 0;
+    }
+
+    struct uart_platform_data_t ut_arg = {0};
+    
+    // TCFG_ONLINE_TX_PORT 和 TCFG_ONLINE_RX_PORT 通常在 app_config.h 中定义
+    ut_arg.tx_pin = TCFG_ONLINE_TX_PORT;
+    ut_arg.rx_pin = (u8)-1; // -1 表示不使用该引脚，因为我们只发送数据
+    ut_arg.baud = baudrate;
+    
+    // 以下为接收相关配置，由于只发送，全部设为0或NULL
+    ut_arg.rx_cbuf = NULL;
+    ut_arg.rx_cbuf_size = 0;
+    ut_arg.frame_length = 0;
+    ut_arg.rx_timeout = 0;
+    ut_arg.isr_cbfun = NULL;
+    
+    ano_uart_dev = (uart_bus_t *)uart_dev_open(&ut_arg);
+
+    if (ano_uart_dev == NULL) {
+        return -1;
+    }
+    #endif
+    return 0;
+
+}
+
+/**
+ * @brief 发送惯性传感器数据 (ID: 0x01)
+ */
+void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z, 
+                            s16 gyr_x, s16 gyr_y, s16 gyr_z, 
+                            u8 shock_sta) {
+    #if TCFG_UART0_ENABLE==0                            
+    if (ano_uart_dev == NULL) {
+        return; // 如果串口未初始化，则不执行任何操作
+    }
+
+    // 帧总长度 = 4(固定头) + 13(数据) + 2(校验) = 19 字节
+    u8 frame_buffer[19];
+    u8 data_idx = 4; // DATA区域从索引4开始
+
+    // 1. 填充帧头、地址、ID、长度
+    frame_buffer[0] = ANO_FRAME_HEADER;
+    frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
+    frame_buffer[2] = 0x01; // 功能码 ID
+    frame_buffer[3] = 13;   // 数据长度 LEN
+
+    // 2. 填充数据内容 (DATA)，注意小端模式 (低字节在前)
+    frame_buffer[data_idx++] = (u8)(acc_x & 0xFF);
+    frame_buffer[data_idx++] = (u8)(acc_x >> 8);
+    frame_buffer[data_idx++] = (u8)(acc_y & 0xFF);
+    frame_buffer[data_idx++] = (u8)(acc_y >> 8);
+    frame_buffer[data_idx++] = (u8)(acc_z & 0xFF);
+    frame_buffer[data_idx++] = (u8)(acc_z >> 8);
+    
+    frame_buffer[data_idx++] = (u8)(gyr_x & 0xFF);
+    frame_buffer[data_idx++] = (u8)(gyr_x >> 8);
+    frame_buffer[data_idx++] = (u8)(gyr_y & 0xFF);
+    frame_buffer[data_idx++] = (u8)(gyr_y >> 8);
+    frame_buffer[data_idx++] = (u8)(gyr_z & 0xFF);
+    frame_buffer[data_idx++] = (u8)(gyr_z >> 8);
+    
+    frame_buffer[data_idx++] = shock_sta;
+
+    // 3. 计算并填充校验和
+    ano_calculate_checksum(frame_buffer);
+
+    // 4. 通过串口发送整个数据帧
+    ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
+    #endif
+}
+
+
+/**
+ * @brief 发送飞控姿态数据 (ID: 0x03)
+ * 
+ * @param rol 
+ * @param pit 
+ * @param yaw 
+ * @param fusion_sta 
+ */
+void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta) {
+#if TCFG_UART0_ENABLE==0
+    if (ano_uart_dev == NULL) {
+        return; // 如果串口未初始化，则不执行任何操作
+    }
+
+    // 帧总长度 = 4(固定头) + 7(数据) + 2(校验) = 13 字节
+    u8 frame_buffer[13];
+    u8 data_idx = 4; // DATA区域从索引4开始
+
+    // 1. 填充帧头、地址、ID、长度
+    frame_buffer[0] = ANO_FRAME_HEADER;
+    frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
+    frame_buffer[2] = 0x03; // 功能码 ID
+    frame_buffer[3] = 7;    // 数据长度 LEN
+
+    // 2. 转换浮点数为整数并填充 (DATA)，注意小端模式
+    s16 rol_int = (s16)(rol * 100);
+    s16 pit_int = (s16)(pit * 100);
+    s16 yaw_int = (s16)(yaw * 100);
+
+    frame_buffer[data_idx++] = (u8)(rol_int & 0xFF);
+    frame_buffer[data_idx++] = (u8)(rol_int >> 8);
+    frame_buffer[data_idx++] = (u8)(pit_int & 0xFF);
+    frame_buffer[data_idx++] = (u8)(pit_int >> 8);
+    frame_buffer[data_idx++] = (u8)(yaw_int & 0xFF);
+    frame_buffer[data_idx++] = (u8)(yaw_int >> 8);
+    
+    frame_buffer[data_idx++] = fusion_sta;
+
+    // 3. 计算并填充校验和
+    ano_calculate_checksum(frame_buffer);
+
+    // 4. 通过串口发送整个数据帧
+    ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
+#endif
+}

apps/earphone/xtell_Sensor/ano/ano_protocol.h

@@ -0,0 +1,37 @@
+#ifndef __ANO_PROTOCOL_H__
+#define __ANO_PROTOCOL_H__
+
+#include "system/includes.h"
+
+/**
+ * @brief 初始化用于上位机通信的串口
+ * 
+ * @param baudrate 波特率，例如 115200
+ * @return 0: 成功, -1: 失败
+ */
+int ano_protocol_init(u32 baudrate);
+
+/**
+ * @brief 发送惯性传感器数据 (ID: 0x01)
+ * @param acc_x X轴加速度
+ * @param acc_y Y轴加速度
+ * @param acc_z Z轴加速度
+ * @param gyr_x X轴陀螺仪
+ * @param gyr_y Y轴陀螺仪
+ * @param gyr_z Z轴陀螺仪
+ * @param shock_sta 震动状态
+ */
+void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z, 
+                            s16 gyr_x, s16 gyr_y, s16 gyr_z, 
+                            u8 shock_sta);
+
+/**
+ * @brief 发送飞控姿态数据 (ID: 0x03)
+ * @param rol 横滚角 (单位: 度)
+ * @param pit 俯仰角 (单位: 度)
+ * @param yaw 航向角 (单位: 度)
+ * @param fusion_sta 融合状态
+ */
+void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta);
+
+#endif // __ANO_PROTOCOL_H__

apps/earphone/xtell_Sensor/calculate/skiing_tracker.c

@@ -3,8 +3,10 @@
 */
 #include "skiing_tracker.h"
 #include "../sensor/SC7U22.h"
-#include <math.h>
+
-#include <string.h>
+
+#define G_ACCELERATION 9.81f
+#define DEG_TO_RAD (3.14159265f / 180.0f)
 
 #define ENABLE_XLOG 1
 #ifdef xlog
@@ -16,12 +18,83 @@
     #define xlog(format, ...) ((void)0)
 #endif
 
+// --- 静止检测 ---
+//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
+// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
+#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
+// 陀螺仪方差阈值
+#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
+// 静止时候的陀螺仪模长
+#define STOP_GYR_MAG_THRESHOLD  15
+// --- --- --- 
 
-#define G_ACCELERATION 9.81f
+// --- 启动滑雪阈值 --- 
-#define DEG_TO_RAD (3.14159265f / 180.0f)
+// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
+#define START_ACC_MAG_THRESHOLD 1.0f  //0.5、1
+// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
+#define START_GYR_VARIANCE_THRESHOLD 15.0f
+// --- --- --- 
 
+// --- 滑雪过程 ---
+//加速度 模长(不含重力），低于此值视为 在做匀速运动
+#define SKIING_ACC_MAG_THRESHOLD 0.5f
+//陀螺仪 模长，高于此值视为 摔倒了
+#define FALLEN_GRY_MAG_THRESHOLD 2000.0f   //未确定
+// --- --- --- 
+
+// --- 原地旋转抖动 --- 
+// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
+#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
+// 加速度 模长 阈值
+#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
+// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
+#define ROTATION_GYR_MAG_THRESHOLD 30.0f  
+// --- --- --- 
+
+// --- 滑雪转弯动 --- 
+// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
+#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f  
+// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
+#define WHEEL_GYR_MAG_THRESHOLD 500.0f   // 
+// --- --- --- 
+
+// --- 跳跃 --- 
+// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
+#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
+// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
+#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
+// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
+#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
+// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
+#define AIRBORNE_CONFIRM_COUNT 3
+// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
+#define GROUNDED_CONFIRM_COUNT 2
+// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
+#define MAX_TIME_IN_AIR 12.5f 
+// --- --- --- 
+
+// --- 用于消除积分漂移的滤波器和阈值 ---
+// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
+// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
+#define HPF_ALPHA 0.999f  
+//0.995f： 0.08 Hz 的信号
+//0.999f： 0.0159 Hz 
+// --- --- --- 
+
+// --- 低通滤波器 ---
+// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
+// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
+#define LPF_ALPHA 0.7f
+
+// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
+// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
+//参考：0.2f ~ 0.4f
+#define ACC_DEAD_ZONE_THRESHOLD 0.05f   
+
+// --- 模拟摩擦力，进行速度衰减 ---
+#define SPEED_ATTENUATION 1.0f  //暂不模拟  
 BLE_KS_send_data_t KS_data;
-
+static float quaternion_data[4];
 #ifdef XTELL_TEST
 
 debug_t debug1;
@@ -81,51 +154,54 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
 
 
 /**
- * @brief 在设备坐标系下，从原始加速度数据中移除重力分量
+ * @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
+ * @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
  * @param acc_device        输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
- * @param angle             输入：姿态角 [pitch, roll, yaw]，单位: 度
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
  * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
  */
-void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
+void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
 {
-    float pitch = angle[0] * DEG_TO_RAD; // 绕 Y 轴
+    // 从四元数计算重力在设备坐标系下的投影
-    float roll  = angle[1] * DEG_TO_RAD;  // 绕 X 轴
+    // G_device = R_transpose * G_world
-    float yaw   = angle[2] * DEG_TO_RAD;  // 绕 Z 轴
+    // G_world = [0, 0, g]
+    // R_transpose 的第三列即为重力投影方向
+    float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
+    float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
+    float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
 
-    float cp = cosf(pitch);
+    // 从原始加速度中减去重力分量
-    float sp = sinf(pitch);
+    acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
-    float cr = cosf(roll);
+    acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
-    float sr = sinf(roll);
+    acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
-    float cy = cosf(yaw);
-    float sy = sinf(yaw);
-
-    // 世界坐标系下的重力矢量
-    const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
-
-    // 计算旋转矩阵 R 的转置矩阵 R_transpose
-    // R (Z-Y-X) = 
-    // [ cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr]
-    // [ sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr]
-    // [ -sp,    cp*sr,             cp*cr            ]
-    //
-    // R_transpose = 
-    // [ cy*cp,              sy*cp,              -sp   ]
-    // [ cy*sp*sr - sy*cr,   sy*sp*sr + cy*cr,   cp*sr ]
-    // [ cy*sp*cr + sy*sr,   sy*sp*cr - cy*sr,   cp*cr ]
-    
-    // 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
-    // 由于 G_world 只有 z 分量，计算可以简化
-    float g_device[3];
-    g_device[0] = (-sp) * g_world[2];
-    g_device[1] = (cp * sr) * g_world[2];
-    g_device[2] = (cp * cr) * g_world[2];
-    
-    // 从原始设备加速度中减去重力投影
-    acc_linear_device[0] = acc_device[0] - g_device[0];
-    acc_linear_device[1] = acc_device[1] - g_device[1];
-    acc_linear_device[2] = acc_device[2] - g_device[2];
 }
 
+/**
+ * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
+ * @details 同样，此方法比使用欧拉角更优。
+ * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
+ * @param q                 输入：表示姿态的四元数 [w, x, y, z]
+ * @param acc_linear_world  输出：世界坐标系下的线性加速度 [x, y, z]
+ */
+void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
+{
+    // 这是 R_device_to_world * acc_linear_device 的展开形式
+    acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
+                          (2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
+
+    acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
+                          (2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
+
+    acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
+                          (2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
+                          (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
+    // acc_linear_world[2] -= G_ACCELERATION;
+}
+
+
+
 /**
  * @brief 计算缓冲区内三轴数据的方差之和
  * 
@@ -161,7 +237,170 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
     return variance[0] + variance[1] + variance[2];
 }
 
+/**
+ * @brief 摩擦力模拟，进行速度衰减
+ * 
+ * @param tracker 
+ */
+void forece_of_friction(skiing_tracker_t *tracker){
+    // 增加速度衰减，模拟摩擦力
+    tracker->velocity[0] *= SPEED_ATTENUATION;
+    tracker->velocity[1] *= SPEED_ATTENUATION;
+    tracker->velocity[2] = 0; // 垂直速度强制归零
+}
 
+/**
+ * @brief 状态机更新
+ * 
+ * @param tracker 传入同步修改后传出
+ * @param acc_device_ms2 三轴加速度，m/s^2
+ * @param gyr_dps 三轴陀螺仪，dps
+ */
+static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
+{
+    // 缓冲区未填满时，不进行状态判断，默认为静止
+    if (!tracker->buffer_filled) {
+        tracker->state = STATIC;
+        return;
+    }
+
+    // --- 计算关键指标 ---
+    float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
+    float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
+    float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);  //dps
+    float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);  //m/s^s
+    float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
+
+    #ifdef XTELL_TEST
+    debug1.acc_variance =acc_variance;
+    debug1.gyr_variance =gyr_variance;
+    debug1.gyr_magnitude=gyr_magnitude;
+    debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
+    #endif
+
+
+
+   // --- 状态机逻辑 (核心修改区域) ---
+
+   #if 0  //暂时不考虑空中
+    // 1. 空中/落地状态的后续处理
+    if (tracker->state == IN_AIR) {
+        // A. 检测巨大冲击 -> 落地
+        if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
+            tracker->state = LANDING;
+        // B. 检测超时 -> 强制落地 (安全机制)
+        } else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
+            tracker->state = LANDING; 
+        // C. 检测恢复正常重力 (平缓落地)
+        } else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
+            tracker->grounded_entry_counter++;
+            if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
+                tracker->state = LANDING;
+            }
+        } else {
+            tracker->grounded_entry_counter = 0;
+        }
+        return; // 在空中或刚切换到落地，结束本次状态判断
+    }
+
+    // 2. 严格的 "起跳->空中" 状态转换逻辑
+    // 只有当处于滑行状态时，才去检测起跳意图
+    if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
+        if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
+            tracker->state = TAKING_OFF;
+            tracker->airborne_entry_counter = 0; // 准备检测失重
+            return;
+        }
+    }
+    // 只有在TAKING_OFF状态下，才去检测是否进入失重
+    if (tracker->state == TAKING_OFF) {
+        if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
+            tracker->airborne_entry_counter++;
+            if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
+                memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
+                tracker->time_in_air = 0;
+                tracker->state = IN_AIR;
+                tracker->airborne_entry_counter = 0;
+                tracker->grounded_entry_counter = 0;
+                return;
+            }
+        } else {
+            // 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
+            // 可以加一个小的超时计数器，这里为了简单先直接恢复
+            tracker->state = NO_CONSTANT_SPEED;
+        }
+        return; // 无论是否切换，都结束本次判断
+    }
+    #endif
+
+    
+    // --- 静止判断 ---
+    if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
+        tracker->state = STATIC;
+        return;
+    }
+
+    // --- 地面状态切换逻辑 ---
+    switch (tracker->state) {
+        case LANDING: 
+            tracker->state = STATIC;
+            break;
+        case STATIC:
+            // 优先判断是否进入 WOBBLE 状态
+            // 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
+            if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
+                tracker->state = WOBBLE;
+            } 
+            // 只有在陀螺仪和加速度都满足“前进”特征时，才启动
+            else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            break;
+
+        case WOBBLE:
+            // 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
+            if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            // 如果陀螺仪活动减弱，则可能恢复静止
+            else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
+                // 不直接跳回STATIC，而是依赖下一轮的全局静止判断
+            }
+            break;
+        case NO_CONSTANT_SPEED:  //非匀速状态
+            //暂时不考虑摔倒
+            // if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
+            //     tracker->state = FALLEN;  //摔倒
+            // } else 
+            if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
+                tracker->state = WHEEL;  //转弯
+            } else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
+                tracker->state = CONSTANT_SPEED;  //匀速
+            }
+            break;
+
+        case CONSTANT_SPEED:  //匀速状态
+            if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { 
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            //TODO：可以添加进入转弯或摔倒的判断
+            break;
+
+        case WHEEL:
+            // 从转弯状态，检查转弯是否结束
+            // 如果角速度和加速度方差都降下来了，就回到普通滑行状态
+            if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
+                tracker->state = NO_CONSTANT_SPEED;
+            }
+            break;
+
+        case FALLEN:
+            // TODO：回到 STATIC
+            break;
+    }
+
+
+}
 
 
 /**
@@ -187,27 +426,140 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
     acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
     acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
 
-    #if 1  //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
+    // 将最新数据存入缓冲区
-    float tmp_device_acc[3];
+    memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
-    extern void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device);
+    memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
-    remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
+    
+    tracker->buffer_index++;
+    if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
+        tracker->buffer_index = 0;
+        tracker->buffer_filled = 1; // 标记缓冲区已满
+    }
+
+    // --- 更新状态机 ---
+    update_state_machine(tracker, acc_device_ms2, gyr_dps);
+
+    // --- 根据状态执行不同的计算逻辑 ---
+    switch (tracker->state) {
+        case TAKING_OFF:
+            tracker->speed = 0.0f;
+            break;
+        case IN_AIR:
+            // 在空中时，只累加滞空时间
+            tracker->time_in_air += dt;
+            break;
+        case LANDING:
+            // 刚落地，计算空中距离
+            calculate_air_distance(tracker);
+            // 清理速度和滤波器状态，为恢复地面追踪做准备
+            memset(tracker->velocity, 0, sizeof(tracker->velocity));
+            tracker->speed = 0;
+            memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
+            memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
+            memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
+            break;
+        case WHEEL:
+        case NO_CONSTANT_SPEED:
+            float linear_acc_device[3];
+            float linear_acc_world[3];
+            // 在设备坐标系下，移除重力，得到线性加速度
+            q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
+
+            // 将设备坐标系下的线性加速度，旋转到世界坐标系
+            q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
+             // 将最终用于积分的加速度存入 tracker 结构体
+            memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
+
+            float acc_world_temp[3]; // 临时变量存储当前周期的加速度
+            for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
+
+                // --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
+
+                // 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
+                //    HPF的瞬态响应会产生尖峰，这是正常的。
+                tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
+                tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
+
+                // 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
+                //    这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
+                tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
+                
+                // 将最终处理完的加速度值存入临时变量
+                acc_world_temp[i] = tracker->acc_world_lpf[i];
+            }
 
             // 计算处理后加速度的水平模长
-    float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
+            float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
-                                        tmp_device_acc[1] * tmp_device_acc[1] + 
+                                                acc_world_temp[1] * acc_world_temp[1]);
-                                        tmp_device_acc[2] * tmp_device_acc[2]);
+            #if XTELL_TEST
+            debug2.acc_magnitude = acc_horizontal_mag;
+            #endif
+            // 应用死区，并积分
+            if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
+                tracker->velocity[0] += acc_world_temp[0] * dt;
+                tracker->velocity[1] += acc_world_temp[1] * dt;
+            }
+
+            // 更新速度和距离
+            tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
+                                tracker->velocity[1] * tracker->velocity[1]);
+            tracker->distance += tracker->speed * dt;
+            break;
+        case CONSTANT_SPEED:
+            //保持上次的速度不变。只更新距离
+            tracker->distance += tracker->speed * dt;
+            break;
+        case STATIC:
+        case WOBBLE:
+            // 速度清零，抑制漂移
+            memset(tracker->velocity, 0, sizeof(tracker->velocity));
+            tracker->speed = 0.0f;
+            memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
+            memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
+            memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
+            #if XTELL_TEST
+            debug2.acc_magnitude = 0;
+            #endif
+            break;   
+        case FALLEN:
+            // TODO
+            break;
+        default:
+            break;
+    }
+
+
+   
+
+    #if 1
+    float linear_acc_device[3];
+    float linear_acc_world[3];
+    float tmp_world_acc[3];
+    // 在设备坐标系下，移除重力，得到线性加速度
+    q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
+
+    // 将设备坐标系下的线性加速度，旋转到世界坐标系
+    q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
+
+    
+    float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
+                                        tmp_world_acc[1] * tmp_world_acc[1] + 
+                                        tmp_world_acc[2] * tmp_world_acc[2]);
+    
     static int count = 0;
     if(count > 100){
         xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
-        xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag);  //去掉重力加速度
+        xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag);  //去掉重力加速度
-        xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]);   //angle
+        xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]);   //angle
         xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
+        xlog("===speed(cm/s): %d\n",(int)(tracker->speed*100) );
         count = 0;
+        
     }
     count++;
-    
     #endif
 
+
 }
 
 
@@ -218,18 +570,21 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
  * @param acc_data_buf 传入的三轴加速度数据
  * @param gyr_data_buf 传入的三轴陀螺仪数据
  * @param angle_data   传入的欧若拉角数据
- * @return BLE_send_data_t 要发送给蓝牙的数据
+ * @return 速度cm/s
  */
-BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
+uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
     
     static int initialized = 0;
     static float acc_data_g[3];
     static float gyr_data_dps[3];
 
+    if(quaternion != NULL){
+        memcpy(quaternion_data, quaternion,  4 * sizeof(float));
+    }
+
     // const float delta_time = DELTA_TIME+0.01f;
     // const float delta_time = DELTA_TIME + 0.005f;
     const float delta_time = DELTA_TIME;
-    BLE_send_data_t BLE_send_data;
     
     if (!initialized) {
         skiing_tracker_init(&my_skiing_tracker);
@@ -274,22 +629,7 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
 
     skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
 
-    // BLE_send_data.skiing_state = my_skiing_tracker.state;
-    // for (int i = 0; i < 3; i++) {
-    //     #ifdef XTELL_TEST
-    //     BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
-    //     BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i];  //dps
-    //     BLE_send_data.angle_data[i] = angle_data[i];
-    //     #else
-    //     BLE_send_data.acc_data[i] = (short)acc_data_buf[i];   //原始adc数据
-    //     BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i];   //原始adc数据
-    //     BLE_send_data.angle_data[i] = angle_data[i];
-    //     #endif
-    // }
-    // BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
-    // BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
-    // // printf("Calculate the time interval =============== end\n");
     
-    return BLE_send_data;
+    return (uint16_t)(my_skiing_tracker.speed * 100);
 }
 

apps/earphone/xtell_Sensor/calculate/skiing_tracker.h

@@ -2,6 +2,10 @@
 #define SKIING_TRACKER_H
 
 #include "../xtell.h"
+#include <math.h>
+#include <string.h>
+
+
 // 定义滑雪者可能的状态
 typedef enum {
     STATIC,                   // 静止或动态稳定：0
@@ -30,7 +34,7 @@ typedef struct {
     skiing_state_t state;       // 当前滑雪状态
 
     // 内部计算使用的私有成员
-    float acc_world[3];         // 在世界坐标系下的加速度
+    float acc_no_g[3];         // 去掉重力分量后的加速度
 
     // 用于空中距离计算
     float time_in_air;                     // 滞空时间计时器
@@ -52,16 +56,7 @@ typedef struct {
     float acc_world_lpf[3];       // 经过低通滤波后的世界坐标系加速度
 } skiing_tracker_t;
 
-//ble发送的数据
+
-typedef struct{  //__attribute__((packed)){   //该结构体取消内存对齐
-    char sensor_state;
-    char skiing_state;
-    int speed_cms;        //求出的速度，cm/s
-    int distance_cm;      //求出的距离，cm
-    short acc_data[3];  //三轴加速度, g
-    short gyr_data[3];  //三轴陀螺仪, dps
-    float angle_data[3];     //欧若拉角
-}BLE_send_data_t;
 
 typedef struct{
     int acc_KS[3];      //卡尔曼后，LSB转换后的  三轴加速度数据（cm/s^2）
@@ -84,5 +79,5 @@ typedef struct{
  */
 void skiing_tracker_init(skiing_tracker_t *tracker);
 
-BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
+uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
 #endif // SKIING_TRACKER_H

apps/earphone/xtell_Sensor/send_data.c

@@ -6,6 +6,8 @@
 #include "tone_player.h"
 #include "ui_manage.h"
 #include "gpio.h"
+#include <math.h>
+#include <string.h>
 #include "app_main.h"
 #include "asm/charge.h"
 #include "update.h"
@@ -20,6 +22,13 @@
 #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"
+#include "asm/rtc.h"
+#include "system/timer.h"
+#include "adv_time_stamp_setting.h"
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 //宏定义
 #define ENABLE_XLOG 1
@@ -31,6 +40,9 @@
 #else
     #define xlog(format, ...) ((void)0)
 #endif
+
+#define SENSOR_DATA_BUFFER_SIZE 1000 // 定义缓冲区可以存储XXX个sensor_data_t元素
+
 //
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -38,462 +50,343 @@
 //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);
+void start_collect_fuc(void);
+void BLE_send_fuc(void);
 //END -- 函数定义
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
 
 //////////////////////////////////////////////////////////////////////////////////////////////////
 //START -- 变量定义
+static u32 timer_offset_ms = 0;
 
-// --- 任务ID ---
+typedef struct {
-static u16 xtell_i2c_test_id;
+    // -- 六轴 -- 
-static u16 collect_data_id;
+    signed short SC7U22_data[6];
-static u16 ble_send_data_id;
+    // -- 磁力计 --
-static u16 sensor_read_data_id;
+    uint8_t mmc5603nj_buffer[9];
-static u16 calculate_data_id;
+    // -- 速度 --
+    uint16_t speed_cms;
+    // -- 气压计 --
+    int adc_P; 
+    int adc_T; 
+    // -- 左/右腿 --
+    uint8_t foot_state;  //1：左脚；2：右脚
+    // -- 时间 --
+    u32 timestamp_ms; 
+
+} BLE_send_data_t;
+
+static int count = 0;
 
 // --- 环形缓冲区 ---
-#define SENSOR_DATA_BUFFER_SIZE 512
+static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
-static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
+BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];
-static circle_buffer_t sensor_cb;
 
-
+// -- 任务id --
-//--- test ---
+u16 SC7U22_calibration_id;
-// 全局变量
+u16 start_collect_fuc_id;
-u16 gsensor_id=0;
+u16 BLE_send_fuc_id;
-u16 test_id=0;
 //END -- 变量定义
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
 
-// /**
+// 重置计时器
-//  * @brief  向匿名上位机发送数据帧
+void reset_ms_timer(void) {
-//  * @param  function_id   功能码 (例如 0x01, 0x03)
+    timer_offset_ms = sys_timer_get_ms();
-//  * @param  data          指向 int16_t 数据数组的指针 (例如加速度、欧拉角)
+    xlog("Timer has been reset.\n");
-//  * @param  data_len      int16_t 数据的个数 (例如发送6轴数据时为6，发送3个欧拉角时为3)
-//  * @param  status_byte   附加的状态字节 (例如 SHOCK_STA 或 FUSION_STA)
-//  */
-// void send_data_anotc(uint8_t function_id, int16_t* data, uint8_t data_len, uint8_t status_byte) {
-//     // 定义一个足够大的缓冲区来构建数据帧
-//     // 最大长度(ID 0x01): 1(HEAD)+1(D_ADDR)+1(ID)+1(LEN)+13(DATA)+1(SC)+1(AC) = 19字节
-//     uint8_t buffer[32]; 
-    
-//     uint8_t data_payload_len = data_len * sizeof(int16_t) + sizeof(uint8_t);
-    
-//     // 1. 填充帧头和地址
-//     buffer[0] = 0xAA; // 帧头 HEAD
-//     buffer[1] = 0xFF; // 目标地址 D_ADDR
-
-//     // 2. 填充功能码和数据长度
-//     buffer[2] = function_id;
-//     buffer[3] = data_payload_len;
-
-//     // 3. 填充数据内容 (DATA)
-//     // 首先使用 memcpy 拷贝主要的 int16_t 数组数据
-//     // &buffer[4] 是数据区的起始地址
-//     memcpy(&buffer[4], (uint8_t*)data, data_len * sizeof(int16_t));
-//     // 然后在数据区末尾填充状态字节
-//     buffer[4 + data_len * sizeof(int16_t)] = status_byte;
-
-//     // 4. 计算校验和 (SC 和 AC)
-//     uint8_t sum_check = 0;
-//     uint8_t add_check = 0;
-    
-//     // SC: 和校验 (从帧头到数据区最后一个字节)
-//     for (int i = 0; i < 4 + data_payload_len; ++i) {
-//         sum_check += buffer[i];
-//     }
-    
-//     // 将SC填充到缓冲区
-//     buffer[4 + data_payload_len] = sum_check;
-    
-//     // AC: 附加校验 (从帧头到SC)
-//     for (int i = 0; i < 4 + data_payload_len + 1; ++i) {
-//         add_check += buffer[i];
-//     }
-
-//     // 将AC填充到缓冲区
-//     buffer[4 + data_payload_len + 1] = add_check;
-
-//     // 5. 发送整个数据帧
-//     uint16_t frame_length = 4 + data_payload_len + 2;
-//     // Serial_Send_Buffer(buffer, frame_length);
-//     for (int i = 0; i < frame_length; ++i) {
-//         // 使用 %c 格式化字符来发送单个字节的原始值
-//         printf("%c", buffer[i]);
-//     }
-//     printf("\n");
-// }
-
-void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
-    char buffer[50];   //一次最多发送50字节
-    u8 len = 0;
-    //AA FF 01 六轴数据 EE
-    //TO DO
-    send_data_to_ble_client(&buffer,len);
-
-
-    //AA FF 02 欧若拉角数据 EE
-    // TO DO
-    send_data_to_ble_client(&buffer,len);
 }
 
-
+// 获取从上次重置后经过的毫秒数
-// 从环形缓冲区读取数据并发送
+u32 get_ms_timer(void) {
-void send_sensor_data_task(void) {
+    return sys_timer_get_ms() - timer_offset_ms;
-    // printf("xtell_ble_send\n");
 }
 
-#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 100 // 定义缓冲区可以存储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];
-} 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要发送的数据
-
-
 /**
- * @brief //读取传感器的数据放进缓冲区
+ * @brief 六轴静态校准
  * 
  */
-void sensor_read_data(){
+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;
-    if(circle_buffer_is_full(&sensor_read)){
+    BLE_send_data_t BLE_send_data_tmp;
-        // xlog("sensor_read_data: read buffer full\n");
+    uint8_t mmc5603nj_buffer[9];
-        return;
+    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;
+    extern u8 foot_init;
+    BLE_send_data_tmp.foot_state = foot_init;
+    BLE_send_data_tmp.timestamp_ms = get_ms_timer();
+
+    // -- 放进缓冲区 --
+    if(circle_buffer_is_full(&BLE_send_buff) == 0){
+        circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
     }
     
-    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 = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
-
-        // static count = 0;
-        // if(count > 100){
-        //     count = 0;
-        //     xlog("SL_SC7U22_Angle_Output status:%d",status);
-        //     xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2],tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
-        // }
-        // 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 = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
-        memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
-        memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
-        circle_buffer_write(&sensor_read, &tmp);
-    }
     // xlog("=======sensor_read_data END\n");
 
+   
 }
 
-void calculate_data(){
+/**
-    // xlog("=======start\n");
+ * @brief ble数据发送函数
-    sensor_data_t tmp;
+ * 
+ */
+void BLE_send_fuc(void){
+    BLE_send_data_t data_to_send;
 
-    if(circle_buffer_is_empty(&sensor_read)){
+    if (circle_buffer_is_empty(&BLE_send_buff) == 0) {
-        // xlog("sensor_read_buffer: read buffer empty\n");
+        circle_buffer_read(&BLE_send_buff, &data_to_send);
+    } else {
+        // 缓冲区为空，直接返回
         return;
     }
 
-    circle_buffer_read(&sensor_read, &tmp);
+    // ---  封装并发送六轴传感器数据 ---
-    BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
+    {
+        // 协议定义: 包头(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;
         
-    if(circle_buffer_is_full(&sensor_send))
+        uint8_t imu_packet[IMU_PACKET_LEN];
-        return;
-    circle_buffer_write(&sensor_send, &data_by_calculate);
 
-    // extern void BLE_send_data();
+        // 填充包头
-    // BLE_send_data();
+        imu_packet[0] = 0xBB;
-    // xlog("=======end\n");
+        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);
     }
     
-static int count = 0;
+    // --- 封装并发送磁力计数据 ---
-extern char xt_Check_Flag;
+    {
-void BLE_send_data(){
+        // 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(9) = 13字节
-    //  xlog("=======start\n");
+        const uint8_t MAG_PACKET_LEN = 13;
-    if(circle_buffer_is_empty(&sensor_send)){
+        const uint8_t MAG_PAYLOAD_LEN = 10; // 类型(1) + 数据(9)
-        // xlog("sensor_send_buffer: send buffer empty\n");
+        const uint8_t MAG_TYPE = 0x02;
-        return;
+
+        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];
         }
-    #ifdef XTELL_TEST
+        // xlog("mag: 0x%x 0x%x 0x%x\n",mag_packet[4],mag_packet[5],mag_packet[6]);
-    // #if 0
-    BLE_send_data_t tmp;
-    circle_buffer_read(&sensor_send, &tmp);
 
-    if(count >=100){ 
+        send_data_to_ble_client(&mag_packet, MAG_PACKET_LEN);
-        // 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);
-        // 使用 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));
-
-        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
+    {
+        // 协议定义: 包头(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;
 
-static u16 xt_iic_test_id;
+        uint8_t pt_packet[PT_PACKET_LEN];
-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));
+        pt_packet[0] = 0xBB;
-    extern char iic_read_len;
+        pt_packet[1] = 0xBE;
-    extern char iic_write_result;
+        pt_packet[2] = PT_PAYLOAD_LEN;
-    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);  
+        pt_packet[3] = PT_TYPE;
-	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)
+        // 打包压力数据 data_to_send.adc_P (占 pt_packet[4] 到 pt_packet[7])
-        send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1));
+        pt_packet[4] = (uint8_t)(data_to_send.adc_P & 0xFF);         // 最低字节 (LSB)
-    if(sen_log_buffer_2 != NULL)
+        pt_packet[5] = (uint8_t)((data_to_send.adc_P >> 8) & 0xFF);  
-        send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2));
+        pt_packet[6] = (uint8_t)((data_to_send.adc_P >> 16) & 0xFF);
-    if(sen_log_buffer_3 != NULL)
+        pt_packet[7] = (uint8_t)((data_to_send.adc_P >> 24) & 0xFF); // 最高字节 (MSB)
-        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)
+        // 打包温度数据 data_to_send.adc_T (占 pt_packet[8] 到 pt_packet[11])
-        send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1));
+        pt_packet[8] = (uint8_t)(data_to_send.adc_T & 0xFF);         // 最低字节 (LSB)
-    if(w_log_buffer_2 != NULL)
+        pt_packet[9] = (uint8_t)((data_to_send.adc_T >> 8) & 0xFF);
-        send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2));
+        pt_packet[10] = (uint8_t)((data_to_send.adc_T >> 16) & 0xFF);
-    if(w_log_buffer_3 != NULL)
+        pt_packet[11] = (uint8_t)((data_to_send.adc_T >> 24) & 0xFF); // 最高字节 (MSB)
-        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();
+        send_data_to_ble_client(&pt_packet, PT_PACKET_LEN);
     }
 
-#endif
 
-#if 0
+    // --- 封装并发送速度数据 ---
-u16 xt_iic_test_id;
+    {
-char hw_iic_init_result;
+        // 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(2) = 6字节
-void xt_hw_iic_test(){
+        const uint8_t SPEED_PACKET_LEN = 6;
-    char log_buffer[100]; 
+        const uint8_t SPEED_PAYLOAD_LEN = 3; // 类型(1) + 数据(2)
-    extern char iic_read_len;
+        const uint8_t SPEED_TYPE = 0x04;
-    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);  
+        uint8_t speed_packet[SPEED_PACKET_LEN];
-	extern void send_data_to_ble_client(const u8* data, u16 length);
+        
-    send_data_to_ble_client(&log_buffer,strlen(log_buffer));
+        // 填充包头
+        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);
     }
-#endif
 
+    // --- 封装并发送数据 ---
+    {
+        // 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(5) = 9字节
+        const uint8_t OTHER_PACKET_LEN = 9;
+        const uint8_t OTHER_PAYLOAD_LEN = 6; // 类型(1) + 数据(5)
+        const uint8_t OTHER_TYPE = 0x05;
+
+        uint8_t oher_packet[OTHER_PACKET_LEN];
+        
+        // 填充包头
+        oher_packet[0] = 0xBB;
+        oher_packet[1] = 0xBE;
+        // 填充长度
+        oher_packet[2] = OTHER_PACKET_LEN;
+        // 填充类型
+        oher_packet[3] = OTHER_TYPE;
+        
+        // 小端模式
+        oher_packet[4] = (uint8_t)data_to_send.foot_state; // 数据来源
+
+        oher_packet[5] = (uint8_t)((data_to_send.timestamp_ms >> 0) & 0xFF);  // LSB
+        oher_packet[6] = (uint8_t)((data_to_send.timestamp_ms >> 8) & 0xFF);
+        oher_packet[7] = (uint8_t)((data_to_send.timestamp_ms >> 16) & 0xFF);
+        oher_packet[8] = (uint8_t)((data_to_send.timestamp_ms >> 24) & 0xFF); // MSB
+        send_data_to_ble_client(&oher_packet, OTHER_PACKET_LEN);
+    }
+}
+// ------------------------------------------------------------------------------------------------------
+// ------------------------------------------------------------------------------------------------------
+
+
+/**
+ * @brief 六轴静态校验
+ * 
+ */
+void start_calibration(void){
+    create_process(&SC7U22_calibration_id,"SC7U22_calibration",NULL,SC7U22_static_calibration,10);
+}
+
+/**
+ * @brief 开始采集传感器数据并通过ble发送
+ * 
+ */
+void start_clloct(void){
+    reset_ms_timer();
+    create_process(&start_collect_fuc_id,"start_collect",NULL,start_collect_fuc,10);
+    create_process(&BLE_send_fuc_id,"BLE_send_fuc",NULL,BLE_send_fuc,1);
+}
+
+/**
+ * @brief 停止采集和ble发送
+ * 
+ */
+void stop_clloct(void){
+    close_process(&start_collect_fuc_id,"start_collect");
+    close_process(&BLE_send_fuc_id,"BLE_send_fuc");
+}
+
+/**
+ * @brief 初始化，在app_main.c的app_main函数被调用
+ * 
+ */
 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
@@ -502,41 +395,46 @@ void xtell_task_create(void){
     
     #endif
     
-    gpio_set_direction(IO_PORTE_05,0);          //设置PE5 输出模式
+    // MPU9250_Mag_Init();
-    gpio_set_pull_up(IO_PORTE_05,1);
+    //iic总线设备扫描
-    gpio_direction_output(IO_PORTE_05,1);
+    // extern void i2c_scanner_probe(void);
-
+    // i2c_scanner_probe();
-    // os_time_dly(10);
-    delay_2ms(10);
-
-    SL_SC7U22_Config();
-    // extern u8 LIS2DH12_Config(void);
-    // LIS2DH12_Config();
 
     xlog("xtell_task_create\n");
-    // 初始化环形缓冲区
+
-    // circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
+    circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
 
 
-    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));
-
-
-    //初始化滑雪追踪器
-    // SkiingTracker_Init(&skiing_data); 
-    xlog("SkiingTracker_Init\n");
-
-
-    create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
-
-    create_process(&calculate_data_id, "calculate",NULL, calculate_data, 6);
-
-    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
 
 }
+
+
+
+//////////////////////////////////////////////////////////////////////////////
+//test
+//
+
+void bmp280_test(void){
+    
+}
+
+
+
+void gsensor_test(void){
+    #if 1
+    if(count >= 5){
+        xlog("==============time============\n");
+        
+        u32 current_time = get_ms_timer();
+        xlog("Time since last reset: %u ms\n", current_time);
+
+        // 为了演示，我们在这里调用重置
+        if (current_time > 5000) { // 比如超过5秒就重置一次
+            reset_ms_timer();
+        }
+
+        count = 0;
+    }
+    count++;
+    #endif
+}

apps/earphone/xtell_Sensor/sensor/AK8963.c

@@ -0,0 +1,133 @@
+
+#include "AK8963.h"
+#include "math.h"
+#include "os/os_api.h"
+#include "../xtell.h"
+#include "printf.h"
+
+// 用于存放从Fuse ROM读取的磁力计灵敏度校准值
+static float mag_asa_x = 1.0f;
+static float mag_asa_y = 1.0f;
+static float mag_asa_z = 1.0f;
+
+// 磁力计在16-bit分辨率下的转换因子 (单位: uT/LSB)
+#define MAG_RAW_TO_UT_FACTOR (4912.0f / 32760.0f)
+
+/**
+ * @brief 初始化MPU9250的磁力计AK8963
+ * @return 0: 成功, 1: MPU9250连接失败, 2: AK8963连接失败
+ */
+u8 MPU9250_Mag_Init(void) {
+
+    u8 temp_data[3];
+
+    // --- 检查 MPU9250 连接并复位 ---
+    _gravity_sensor_get_ndata(MPU9250_ADDR_R, MPU9250_WHO_AM_I, temp_data, 1);
+    if (temp_data[0] != 0x71 && temp_data[0] != 0x73) {
+        printf("MPU9250 comm failed, read ID: 0x%X\n", temp_data[0]);
+        return 1;
+    }
+    printf("MPU9250 get id:0x%X\n", temp_data[0]);
+
+    gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x80); // 软复位
+    os_time_dly(10); // 等待复位完成
+
+    gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x01); // 退出睡眠，选择时钟源
+    os_time_dly(2);
+
+    // --- 强制复位 I2C Master 模块并开启旁路 ---
+    
+    gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x20); 
+    os_time_dly(1);
+    gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x00);
+    os_time_dly(1);
+    
+    gravity_sensor_command(MPU9250_ADDR_W, MPU9250_INT_PIN_CFG, 0x02);
+    os_time_dly(2);
+
+    // --- 再次验证 AK8963 连接 ---
+    _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_WIA, temp_data, 1);
+    if (temp_data[0] != 0x48) {
+        printf("AK8963 comm failed after final attempt, read ID: 0x%X\n", temp_data[0]);
+        return 2;
+    }
+    printf("AK8963 get id: 0x%X\n", temp_data[0]);
+
+    // ------------------ 配置 AK8963 ------------------
+    // Power-down模式
+    gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
+    os_time_dly(1);
+
+    //  Fuse ROM access模式
+    gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x0F);
+    os_time_dly(1);
+    _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ASAX, temp_data, 3);
+    
+    // 计算校准系数
+    mag_asa_x = (float)(temp_data[0] - 128) / 256.0f + 1.0f;
+    mag_asa_y = (float)(temp_data[1] - 128) / 256.0f + 1.0f;
+    mag_asa_z = (float)(temp_data[2] - 128) / 256.0f + 1.0f;
+
+    //  再次进入Power-down模式
+    gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
+    os_time_dly(1);
+
+    //  设置工作模式：16-bit分辨率，100Hz连续测量模式 (0x16)
+    gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x16);
+    os_time_dly(1);
+
+    printf("AK8963 configured successfully.\n");
+    return 0; // 初始化成功
+}
+
+
+/**
+ * @brief 读取磁力计的三轴原始数据
+ * @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (int16_t类型)
+ * @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
+ */
+u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz) {
+    u8 read_buf[7];
+
+    // 检查数据是否准备好 (使用8位读地址)
+    _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ST1, read_buf, 1);
+    if (!(read_buf[0] & 0x01)) {
+        return 1; // 数据未就绪
+    }
+
+    // 连续读取7个字节 (使用8位读地址)
+    _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_HXL, read_buf, 7);
+
+    // 检查数据是否溢出
+    if (read_buf[6] & 0x08) {
+        return 2; // 数据溢出
+    }
+
+    // 组合数据
+    *mx = (int16_t)((read_buf[1] << 8) | read_buf[0]);
+    *my = (int16_t)((read_buf[3] << 8) | read_buf[2]);
+    *mz = (int16_t)((read_buf[5] << 8) | read_buf[4]);
+    
+    return 0; // 读取成功
+}
+
+/**
+ * @brief 读取磁力计的三轴数据，并转换为uT(微特斯拉) (此函数内部逻辑不变)
+ * @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (float类型)
+ * @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
+ */
+u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz) {
+    int16_t raw_mx, raw_my, raw_mz;
+    
+    u8 status = MPU9250_Read_Mag_Raw(&raw_mx, &raw_my, &raw_mz);
+    if (status != 0) {
+        return status;
+    }
+    
+    // 应用灵敏度校准，并转换为uT单位
+    *mx = (float)raw_mx * mag_asa_x * MAG_RAW_TO_UT_FACTOR;
+    *my = (float)raw_my * mag_asa_y * MAG_RAW_TO_UT_FACTOR;
+    *mz = (float)raw_mz * mag_asa_z * MAG_RAW_TO_UT_FACTOR;
+
+    return 0;
+}

apps/earphone/xtell_Sensor/sensor/AK8963.h

@@ -0,0 +1,46 @@
+// mpu9250_mag.h
+
+#ifndef __MPU9250_MAG_H
+#define __MPU9250_MAG_H
+
+#include "stdint.h" // 假设你有标准整数类型，u8 对应 uint8_t
+#include "gSensor/gSensor_manage.h"
+
+//==================================================================================
+// MPU9250 和 AK8963 的 I2C 地址 (已转换为8位格式)
+//==================================================================================
+// MPU9250的7位地址是 0x68(接地)
+#define MPU9250_ADDR_7BIT 0x69
+#define MPU9250_ADDR_W        (MPU9250_ADDR_7BIT << 1 | 0) // 8位写地址: 0xD0
+#define MPU9250_ADDR_R        (MPU9250_ADDR_7BIT << 1 | 1) // 8位读地址: 0xD1
+
+// AK8963磁力计的7位地址是 0x0C
+#define AK8963_ADDR_7BIT 0x0C
+#define AK8963_ADDR_W         (AK8963_ADDR_7BIT << 1 | 0) // 8位写地址: 0x18
+#define AK8963_ADDR_R         (AK8963_ADDR_7BIT << 1 | 1) // 8位读地址: 0x19
+
+
+//==================================================================================
+// MPU9250 相关寄存器 (用于开启旁路模式)
+//==================================================================================
+#define MPU9250_WHO_AM_I    0x75
+#define MPU9250_INT_PIN_CFG 0x37
+#define MPU9250_USER_CTRL   0x6A
+#define MPU9250_PWR_MGMT_1  0x6B
+//==================================================================================
+// AK8963 磁力计相关寄存器
+//==================================================================================
+#define AK8963_WIA          0x00
+#define AK8963_ST1          0x02
+#define AK8963_HXL          0x03
+#define AK8963_ST2          0x09
+#define AK8963_CNTL1        0x0A
+#define AK8963_ASAX         0x10
+
+
+u8 MPU9250_Mag_Init(void);
+u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz);
+u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz);
+
+
+#endif // __MPU9250_MAG_H

apps/earphone/xtell_Sensor/sensor/BMP280.c

@@ -0,0 +1,211 @@
+/*
+    气压计
+*/
+#include "BMP280.h"
+#include <string.h> 
+#include "os/os_api.h"
+#include "gSensor/gSensor_manage.h"
+
+/*==================================================================================*/
+/* BMP280 内部定义                                                               */
+/*==================================================================================*/
+
+// 存储校准参数的静态全局变量
+static uint16_t t1;
+static int16_t  t2, t3;
+static uint16_t p1;
+static int16_t  p2, p3, p4, p5, p6, p7, p8, p9;
+static int32_t  t_fine;
+
+/*==================================================================================*/
+/* 封装的底层I2C读写函数                                                         */
+/*==================================================================================*/
+
+/**
+ * @brief 写入单个字节到BMP280寄存器
+ */
+static uint8_t bmp280_write_reg(uint8_t reg, uint8_t data) {
+    gravity_sensor_command(BMP_IIC_WRITE_ADDRESS, reg, data);
+    return 0; 
+}
+
+/**
+ * @brief 从BMP280读取多个字节
+ */
+static uint8_t bmp280_read_regs(uint8_t reg, uint8_t *buf, uint16_t len) {
+    return _gravity_sensor_get_ndata(BMP_IIC_READ_ADDRESS, reg, buf, len);
+}
+
+/*==================================================================================*/
+/* 核心算法                                                                          */
+/*==================================================================================*/
+
+/**
+ * @brief  温度补偿计算
+ * @param  adc_T - 原始温度数据
+ * @return 补偿后的温度值 (单位: °C)
+ */
+static float compensate_temperature(int32_t adc_T) {
+    float var1, var2, temperature;
+
+    var1 = (((float)adc_T) / 16384.0f - ((float)t1) / 1024.0f) * ((float)t2);
+    var2 = ((((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f) *
+           (((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f)) *
+           ((float)t3);
+    t_fine = (int32_t)(var1 + var2);
+    temperature = (var1 + var2) / 5120.0f;
+
+    if (temperature < -40.0f) return -40.0f;
+    if (temperature > 85.0f) return 85.0f;
+    
+    return temperature;
+}
+
+/**
+ * @brief  气压补偿计算
+ * @param  adc_P - 原始气压数据
+ * @return 补偿后的气压值 (单位: Pa)
+ */
+static float compensate_pressure(int32_t adc_P) {
+    float var1, var2, pressure;
+
+    var1 = ((float)t_fine / 2.0f) - 64000.0f;
+    var2 = var1 * var1 * ((float)p6) / 32768.0f;
+    var2 = var2 + var1 * ((float)p5) * 2.0f;
+    var2 = (var2 / 4.0f) + (((float)p4) * 65536.0f);
+    var1 = (((float)p3) * var1 * var1 / 524288.0f + ((float)p2) * var1) / 524288.0f;
+    var1 = (1.0f + var1 / 32768.0f) * ((float)p1);
+
+    if (var1 == 0.0f) {
+        return 0; // 避免除以零
+    }
+
+    pressure = 1048576.0f - (float)adc_P;
+    pressure = (pressure - (var2 / 4096.0f)) * 6250.0f / var1;
+    var1 = ((float)p9) * pressure * pressure / 2147483648.0f;
+    var2 = pressure * ((float)p8) / 32768.0f;
+    pressure = pressure + (var1 + var2 + ((float)p7)) / 16.0f;
+
+    if (pressure < 30000.0f) return 30000.0f;
+    if (pressure > 110000.0f) return 110000.0f;
+
+    return pressure;
+}
+
+/*==================================================================================*/
+/* 外部接口函数实现                                                              */
+/*==================================================================================*/
+
+
+
+
+uint8_t bmp280_init(void) {
+    uint8_t id;
+    uint8_t calib_data[24];
+
+    // 1. 检查芯片ID
+    if (bmp280_read_regs(BMP280_REG_ID, &id, 1) == 0) {
+        printf("bmp280 get id error:%d\n",id );            
+        return 1; // I2C读取失败
+    }
+    if (id != 0x58) {
+        printf("bmp280 check diff:%d\n",id );
+        return 1; // ID不匹配
+    }
+    printf("bmp280 get id:0%X\n",id );  
+
+    // 2. 软复位
+    bmp280_write_reg(BMP280_REG_RESET, 0xB6);
+    os_time_dly(10); // 等待复位完成
+
+    // 3. 一次性读取所有校准参数
+    if (bmp280_read_regs(BMP280_REG_CALIB_START, calib_data, 24) == 0) {
+        return 2; // 读取校准数据失败
+    }
+
+    // 4. 解析校准参数
+    t1 = (uint16_t)(((uint16_t)calib_data[1] << 8) | calib_data[0]);
+    t2 = (int16_t)(((int16_t)calib_data[3] << 8) | calib_data[2]);
+    t3 = (int16_t)(((int16_t)calib_data[5] << 8) | calib_data[4]);
+    p1 = (uint16_t)(((uint16_t)calib_data[7] << 8) | calib_data[6]);
+    p2 = (int16_t)(((int16_t)calib_data[9] << 8) | calib_data[8]);
+    p3 = (int16_t)(((int16_t)calib_data[11] << 8) | calib_data[10]);
+    p4 = (int16_t)(((int16_t)calib_data[13] << 8) | calib_data[12]);
+    p5 = (int16_t)(((int16_t)calib_data[15] << 8) | calib_data[14]);
+    p6 = (int16_t)(((int16_t)calib_data[17] << 8) | calib_data[16]);
+    p7 = (int16_t)(((int16_t)calib_data[19] << 8) | calib_data[18]);
+    p8 = (int16_t)(((int16_t)calib_data[21] << 8) | calib_data[20]);
+    p9 = (int16_t)(((int16_t)calib_data[23] << 8) | calib_data[22]);
+
+    // 5. 配置传感器 (推荐设置: 正常模式，高精度)
+    // t_standby=0.5ms, filter=16, spi_en=0
+    uint8_t config_reg = (0 << 5) | (4 << 2) | (0 << 0);
+    bmp280_write_reg(BMP280_REG_CONFIG, config_reg);
+    
+    // osrs_t=x2, osrs_p=x16, mode=normal
+    uint8_t ctrl_meas_reg = (2 << 5) | (5 << 2) | (3 << 0);
+    bmp280_write_reg(BMP280_REG_CTRL_MEAS, ctrl_meas_reg);
+
+    os_time_dly(10); // 等待配置生效
+
+    printf("bmp280 init success\n");
+    return 0; // 初始化成功
+}
+
+/**
+ * @brief 获取转换后的温度和压力数据
+ * 
+ * @param temperature 传出，温度
+ * @param pressure 传出，压力
+ * @return uint8_t 
+ */
+uint8_t bmp280_read_data(float *temperature, float *pressure) {
+    uint8_t data[6];
+    int32_t adc_P, adc_T;
+
+    // printf("==========debug1===========\n");
+    // 一次性读取6个字节的温度和气压原始数据
+    if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) == 0) {
+        printf("bmp280:read data error\n");
+        return 1; // 读取失败
+    }
+
+    // printf("==========debug2===========\n");
+    // 组合原始数据 (20位)
+    adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
+    adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
+
+    // 如果没有数据，直接返回错误 (ADC读数为0x80000是未测量状态)
+    if (adc_T == 0x80000 || adc_P == 0x80000) {
+        *temperature = 0.0f;
+        *pressure = 0.0f;
+        printf("bmp280:no data\n");
+        return 1;
+    }
+
+    // printf("==========debug3===========\n");
+    // 进行补偿计算
+    *temperature = compensate_temperature(adc_T);
+    *pressure = compensate_pressure(adc_P);
+
+    return 0; // 成功
+}
+
+/**
+ * @brief 获取该气压计的原始adc数据
+ * 
+ * @param adc_P 传出，气压
+ * @param adc_T 传出，温度
+ */
+void bmp280_read_originanl_data(int* adc_P, int* adc_T){
+    uint8_t data[6];
+    // 一次性读取6个字节的温度和气压原始数据
+    if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
+        return; // 读取失败
+    }
+
+    // 组合原始数据 (20位)
+    adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
+    adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
+
+}

apps/earphone/xtell_Sensor/sensor/BMP280.h

@@ -0,0 +1,54 @@
+#ifndef BMP280_DRIVER_H
+#define BMP280_DRIVER_H
+
+#include <stdint.h>
+
+
+#define BMP_PULL_UP 0  //外部是否接的上拉
+
+// I2C 从设备地址
+#if BMP_PULL_UP == 1   //外部接的高
+#define BMP_IIC_7BIT_ADDRESS 0x76 //7位,外部接高为0x77
+#define BMP_IIC_WRITE_ADDRESS  (BMP_IIC_7BIT_ADDRESS<<1)  //8位地址
+#define BMP_IIC_READ_ADDRESS   (BMP_IIC_WRITE_ADDRESS | 0x01)
+#else
+#define BMP_IIC_7BIT_ADDRESS 0x77  //7位,外部接低为0x76
+#define BMP_IIC_WRITE_ADDRESS  (BMP_IIC_7BIT_ADDRESS<<1)  //8位地址
+#define BMP_IIC_READ_ADDRESS   (BMP_IIC_WRITE_ADDRESS | 0x01)
+#endif
+
+
+
+// BMP280 寄存器地址
+#define BMP280_REG_CALIB_START  0x88
+#define BMP280_REG_ID           0xD0
+#define BMP280_REG_RESET        0xE0
+#define BMP280_REG_STATUS       0xF3
+#define BMP280_REG_CTRL_MEAS    0xF4
+#define BMP280_REG_CONFIG       0xF5
+#define BMP280_REG_PRESS_MSB    0xF7
+
+/**
+ * @brief 初始化BMP280传感器
+ * @return 0: 成功, 1: 芯片ID错误, 2: 读取校准参数失败
+ * @note   此函数会完成ID检查、软复位、读取校准参数，并设置传感器为连续测量模式。
+ */
+uint8_t bmp280_init(void);
+
+/**
+ * @brief 从BMP280读取温度和气压数据
+ * @param[out] temperature - 指向浮点数变量的指针，用于存储温度值 (单位: °C)
+ * @param[out] pressure - 指向浮点数变量的指针，用于存储气压值 (单位: Pa)
+ * @return 0: 成功, 1: 读取数据失败
+ */
+uint8_t bmp280_read_data(float *temperature, float *pressure);
+
+/**
+ * @brief 获取该气压计的原始adc数据
+ * 
+ * @param adc_P 传出，气压
+ * @param adc_T 传出，温度
+ */
+void bmp280_read_originanl_data(int* adc_P, int* adc_T);
+
+#endif // BMP280_DRIVER_H

apps/earphone/xtell_Sensor/sensor/MMC56.c

@@ -0,0 +1,259 @@
+
+#include "MMC56.h"
+#include "math.h"
+#include "os/os_api.h"
+#include "../xtell.h"
+#include "gSensor/gSensor_manage.h"
+#include "printf.h"
+
+#define CALIBRATION_TIME 20000  //校准持续时间 ms
+#define SAMPLE_INTERVAL  100    //校准采样间隔
+
+// 用于跟踪当前是否处于连续测量模式
+static uint8_t g_continuous_mode_enabled = 0;
+mmc5603nj_cal_data_t cal_data;  //校准数据
+
+static void mmc5603nj_write_reg(uint8_t reg, uint8_t data) {
+    gravity_sensor_command(MMC_IIC_WRITE_ADDRESS, reg, data);
+}
+static uint32_t mmc5603nj_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
+    return _gravity_sensor_get_ndata(MMC_IIC_READ_ADDRESS, reg, buf, len);
+}
+
+// 外部接口函数实现
+
+uint8_t mmc5603nj_get_pid(void) {
+    uint8_t pid = 0;
+    mmc5603nj_read_regs(MMC_PID, &pid, 1);
+    return pid;
+}
+
+int mmc5603nj_init(void) {
+    //  ID
+    if ( mmc5603nj_get_pid() != 0x10) {
+        printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
+        // return 0;
+    }
+
+    //  软件复位
+    mmc5603nj_write_reg(MMC_INCTRL1, 0x80); // SW_RESET bit
+    os_time_dly(20); // 等待复位完成
+
+    //  设置20位分辨率 (BW[1:0] = 11)
+    //    同时确保所有轴都使能 (X/Y/Z_inhibit = 0)
+    mmc5603nj_write_reg(MMC_INCTRL1, 0x03);
+    os_time_dly(1);
+
+    //  设置内部控制寄存器2
+    //    CMM_EN = 1 (使能连续模式功能)
+    //    HPOWER = 1 (高功耗模式，更稳定)
+    mmc5603nj_write_reg(MMC_INCTRL2, 0x90); // 0b10010000
+
+    //  设置自动SET/RESET功能
+    //    AUTO_SR_EN = 1
+    mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 0b00100000
+
+    g_continuous_mode_enabled = 0;
+    printf("MMC5603NJ initialized successfully.\n");
+
+    mmc5603nj_enable_continuous_mode(0x04);
+
+    return 1;
+}
+
+void mmc5603nj_start_calibration(void){
+    printf("\n--- Magnetometer Calibration Start ---\n");
+    printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n");
+    printf("Calibration will last for 20 seconds.\n\n");
+    printf("will start after 5 seconds\n\n");
+    os_time_dly(500);
+
+    // 初始化最大最小值
+    // 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑
+    mmc5603nj_mag_data_t temp_mag_data;
+    // 首次读取以获取初始值
+    mmc5603nj_read_mag_data(&temp_mag_data); // 首次读取不应用校准
+
+    float max_x = temp_mag_data.x;
+    float min_x = temp_mag_data.x;
+    float max_y = temp_mag_data.y;
+    float min_y = temp_mag_data.y;
+    float max_z = temp_mag_data.z;
+    float min_z = temp_mag_data.z;
+
+    uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳
+    int samples = 0;
+    int over = CALIBRATION_TIME/SAMPLE_INTERVAL;
+
+    while (samples <= over) {
+        // 读取原始磁力计数据
+        mmc5603nj_read_mag_data(&temp_mag_data);
+
+        // 更新最大最小值
+        if (temp_mag_data.x > max_x) max_x = temp_mag_data.x;
+        if (temp_mag_data.x < min_x) min_x = temp_mag_data.x;
+
+        if (temp_mag_data.y > max_y) max_y = temp_mag_data.y;
+        if (temp_mag_data.y < min_y) min_y = temp_mag_data.y;
+
+        if (temp_mag_data.z > max_z) max_z = temp_mag_data.z;
+        if (temp_mag_data.z < min_z) min_z = temp_mag_data.z;
+        
+        samples++;
+        os_time_dly(SAMPLE_INTERVAL / 10); 
+    }
+
+    // 检查数据范围是否合理，防止传感器未动或故障
+    if ((max_x - min_x < 0.1f) || (max_y - min_y < 0.1f) || (max_z - min_z < 0.1f)) {
+        printf("\n--- Calibration Failed ---\n");
+        printf("Device might not have been rotated enough.\n");
+        printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z);
+        return;
+    }
+
+    // 计算硬磁偏移 (椭球中心)
+    cal_data.offset_x = (max_x + min_x) / 2.0f;
+    cal_data.offset_y = (max_y + min_y) / 2.0f;
+    cal_data.offset_z = (max_z + min_z) / 2.0f;
+
+    printf("\n--- Calibration Complete ---\n");
+    printf("Collected %d samples.\n", samples);
+    printf("Offsets (Gauss):\n");
+    printf("  X: %.4f\n", cal_data.offset_x);
+    printf("  Y: %.4f\n", cal_data.offset_y);
+    printf("  Z: %.4f\n", cal_data.offset_z);
+    printf("Please save these values and apply them in your code.\n\n");
+}
+
+
+
+void mmc5603nj_enable_continuous_mode(uint8_t rate) {
+    // 在连续模式下，ODR寄存器必须被设置
+    mmc5603nj_write_reg(MMC_ODR, rate); //要设置频率
+    // mmc5603nj_set_data_rate(0x04);
+
+    // 启用连续模式 (INCTRL2的CMM_EN位已在init中设置)
+    // 只需要设置 INCTRL0 的 CMM_FREQ_EN 位
+    mmc5603nj_write_reg(MMC_INCTRL0, 0xA0); // 0b10100000 (CMM_FREQ_EN=1, AUTO_SR_EN=1)
+    g_continuous_mode_enabled = 1;
+}
+
+void mmc5603nj_disable_continuous_mode(void) {
+    // 禁用连续模式
+    mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 恢复到仅使能 AUTO_SR_EN 的状态
+    g_continuous_mode_enabled = 0;
+}
+
+float mmc5603nj_get_temperature(void) {
+    uint8_t status = 0;
+    uint8_t temp_raw = 0;
+    uint8_t timeout = 20; 
+
+    // 触发一次温度测量
+    mmc5603nj_write_reg(MMC_INCTRL0, 0x02); // TAKE_MEAS_T
+
+    // 等待测量完成
+    do {
+        os_time_dly(10);
+        mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
+        timeout--;
+    } while ((status & 0x80) == 0 && timeout > 0);
+
+    if (timeout == 0) {
+        printf("Error: Temperature measurement timeout!\n");
+        return -273.15f; // 返回一个绝对零度的错误值
+    }
+
+    mmc5603nj_read_regs(MMC_TOUT, &temp_raw, 1);
+    return ((float)temp_raw * 0.8f) - 75.0f;
+}
+
+void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) {
+    uint8_t buffer[9];
+
+    if (g_continuous_mode_enabled) {
+        // 连续模式下，只需检查数据是否就绪
+        uint8_t status = 0;
+        mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
+        if ((status & 0x40) == 0) { // Meas_M_done bit
+            // 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
+            return;
+        }
+    } else {
+        // 单次测量模式
+        uint8_t status = 0;
+        uint8_t timeout = 20;
+
+        // 触发一次带自动SET/RESET的磁场测量
+        mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
+
+        // 等待测量完成
+        do {
+            os_time_dly(10);
+            mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
+            timeout--;
+        } while ((status & 0x40) == 0 && timeout > 0);
+
+        if (timeout == 0) {
+            printf("Error: Magnetic measurement timeout!\n");
+            mag_data->x = mag_data->y = mag_data->z = 0.0f;
+            return;
+        }
+    }
+
+    // 读取9个字节的原始数据
+    mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
+
+    // 解析数据 (20位分辨率)
+    int32_t raw_x = ((uint32_t)buffer[0] << 12) | ((uint32_t)buffer[1] << 4) | ((uint32_t)buffer[6] & 0x0F);
+    int32_t raw_y = ((uint32_t)buffer[2] << 12) | ((uint32_t)buffer[3] << 4) | ((uint32_t)buffer[6] >> 4);
+    int32_t raw_z = ((uint32_t)buffer[4] << 12) | ((uint32_t)buffer[5] << 4) | ((uint32_t)buffer[8] & 0x0F);
+
+    // 应用偏置和灵敏度进行转换
+    mag_data->x = ((float)raw_x - 524288.0f) / 16384.0f;
+    mag_data->y = ((float)raw_y - 524288.0f) / 16384.0f;
+    mag_data->z = ((float)raw_z - 524288.0f) / 16384.0f;
+
+    //减去偏移
+    mag_data->x -= cal_data.offset_x;
+    mag_data->y -= cal_data.offset_y;
+    mag_data->z -= cal_data.offset_z;
+}
+
+
+void mmc5603nj_read_origin_data(uint8_t *buffer) {
+
+    if (g_continuous_mode_enabled) {
+        // 连续模式下，只需检查数据是否就绪
+        uint8_t status = 0;
+        mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
+        if ((status & 0x40) == 0) { // Meas_M_done bit
+            // 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
+            return;
+        }
+    } else {
+        // 单次测量模式
+        uint8_t status = 0;
+        uint8_t timeout = 20;
+
+        // 触发一次带自动SET/RESET的磁场测量
+        mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
+
+        // 等待测量完成
+        do {
+            os_time_dly(10);
+            mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
+            timeout--;
+        } while ((status & 0x40) == 0 && timeout > 0);
+
+        if (timeout == 0) {
+            printf("Error: Magnetic measurement timeout!\n");
+            return;
+        }
+    }
+
+    // 读取9个字节的原始数据
+    mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
+
+
+}

apps/earphone/xtell_Sensor/sensor/MMC56.h

@@ -0,0 +1,103 @@
+#ifndef MMC5603NJ_DRIVER_H
+#define MMC5603NJ_DRIVER_H
+
+#include <stdint.h>
+
+
+//该芯片的iic地址是固定的, 没法通过外部上下拉来改变
+#define BMP_IIC_7BIT_ADDRESS 0x30  //0110000   手册第12页
+//8位地址:
+#define MMC_IIC_WRITE_ADDRESS  (BMP_IIC_7BIT_ADDRESS <<1)    // 0x60 : 01100000
+#define MMC_IIC_READ_ADDRESS   (MMC_IIC_WRITE_ADDRESS | 0x01)  // 0x61 : 01100001
+
+
+
+// 寄存器地址定义 -- 数据手册第6页
+#define MMC_XOUT0               0x00
+#define MMC_XOUT1               0x01
+#define MMC_YOUT0               0x02
+#define MMC_YOUT1               0x03
+#define MMC_ZOUT0               0x04
+#define MMC_ZOUT1               0x05
+#define MMC_XOUT2               0x06
+#define MMC_YOUT2               0x07
+#define MMC_ZOUT2               0x08
+#define MMC_TOUT                0x09
+#define MMC_STATUS1             0x18
+#define MMC_ODR                 0x1A
+#define MMC_INCTRL0             0x1B
+#define MMC_INCTRL1             0x1C
+#define MMC_INCTRL2             0x1D
+#define MMC_ST_X_TH             0x1E
+#define MMC_ST_Y_TH             0x1F
+#define MMC_ST_Z_TH             0x20
+#define MMC_ST_X                0x27
+#define MMC_ST_Y                0x28
+#define MMC_ST_Z                0x29
+#define MMC_PID                 0x39
+
+// 定义一个结构体来存放三轴磁场数据（原始数据）
+typedef struct {
+    float x;
+    float y;
+    float z;
+} mmc5603nj_original_data_t;
+
+
+// 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss）
+typedef struct {
+    float x;
+    float y;
+    float z;
+} mmc5603nj_mag_data_t;
+
+// 定义一个结构体来存放磁力计的硬磁偏移校准数据
+typedef struct {
+    float offset_x;
+    float offset_y;
+    float offset_z;
+} mmc5603nj_cal_data_t;
+
+/**
+ * @brief 初始化MMC5603NJ传感器
+ *        该函数会对传感器进行软件复位，并检查设备ID。
+ * @return 0 表示成功, -1 表示失败 (设备ID不匹配).
+ */
+int mmc5603nj_init(void);
+
+/**
+ * @brief 设置传感器的数据输出速率 (ODR - Output Data Rate)
+ * @param rate 速率值，具体含义请参考datasheet ODR寄存器说明。
+ */
+void mmc5603nj_set_data_rate(uint8_t rate);
+
+/**
+ * @brief 启用连续测量模式
+ */
+void mmc5603nj_enable_continuous_mode(uint8_t rate);
+
+/**
+ * @brief 禁用连续测量模式
+ */
+void mmc5603nj_disable_continuous_mode(void);
+
+/**
+ * @brief 获取产品ID
+ * @return 产品的ID值，对于MMC5603NJ，应为0x10.
+ */
+uint8_t mmc5603nj_get_pid(void);
+
+/**
+ * @brief 读取传感器的温度
+ * @return 温度值 (单位: 摄氏度 °C).
+ */
+float mmc5603nj_get_temperature(void);
+
+/**
+ * @brief 读取三轴磁场数据
+ *        此函数会根据当前是连续模式还是单次模式来读取数据。
+ * @param mag_data 指向 mmc5603nj_mag_data_t 结构体的指针，用于存放结果。
+ */
+void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data);
+
+#endif // MMC5603NJ_DRIVER_H

apps/earphone/xtell_Sensor/sensor/SC7U22.c

@@ -1,10 +1,12 @@
-
+/*
+    六轴
+*/
 #include "SC7U22.h"
 #include "math.h"
 #include "os/os_api.h"
 #include "../xtell.h"
 
-// #define ENABLE_XLOG 1
+#define ENABLE_XLOG 1
 #ifdef xlog
 #undef xlog
 #endif
@@ -68,9 +70,12 @@ char iic_write_result;
 unsigned char SL_SC7U22_Check(void)
 {
 	unsigned char reg_value=0;
-	xlog("SL_SC7U22_Check\n");
+	
 	iic_write_result = SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
+    // xlog("SL_SC7U22_Check write: %d\n", iic_write_result);
+    
 	iic_read_len = SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
+    // xlog("SL_SC7U22_Check read : %d\n", iic_write_result);
 	xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
 	if(reg_value==0x6A)  //设备的id
 		return 0x01;//SC7U22
@@ -129,6 +134,8 @@ unsigned char SL_SC7U22_Config(void)
 		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式，1600Hz -- lmx
 		// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx
 
+		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//ACC_CON 高性能模式，100Hz，平均数4 -- lmx
+
 		#if ACC_RANGE==2
 		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE  00：±2G
 		#endif
@@ -144,8 +151,9 @@ unsigned char SL_SC7U22_Config(void)
 
 		// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
 		// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz  -- lmx
-		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz  -- lmx
+		// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz  -- lmx
 		// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz  -- lmx
+		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xE8);//GYR_CONF 100Hz, 噪声优化开启,4个平均一次  -- lmx
 
 		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
 		SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
@@ -270,7 +278,7 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
 	gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256)  + ((unsigned char)raw_data[9]));//GYRY-16位
 	gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
 	
-	xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
+	// xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
 	
 }
 #else
@@ -997,13 +1005,13 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
 
     return 2; // 校准未完成，返回错误状态
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+//
 /**
  * @brief 姿态角解算函数 (基于一阶互补滤波)
  * @details
- * 该函数主要完成两项工作：
- * 1.  静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
- * 2.  姿态解算：使用一阶互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
- *
  * @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
  * @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
  * @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。
@@ -1159,79 +1167,817 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
     
     return 2; // 校准未完成，返回错误状态
 }
-unsigned char get_calibration_state(void){
+
+unsigned char get_SC7U22_Error_Flag(void){
 	return SL_SC7U22_Error_Flag;
 }
 
+/**
+ * @brief 设置零漂检测标准位
+ * 
+ * @param flag 0：重新进行零漂检测
+ */
+void set_SC7U22_Error_Flag(char flag){
+	SL_SC7U22_Error_Flag = flag;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// =================================================================================================
+// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义
+// 通过一个PI控制器来校正陀ar螺仪的积分漂移。
+// -------------------------------------------------------------------------------------------------
+// --- 滤波器参数 ---
+// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。
+// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。
+// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
+#define HAVE_MAG 0
+#if HAVE_MAG == 0
+// -- 无地磁 --
+const float Kp = 2.0f;
+const float Ki = 0.005f;
+const float Q_dt = 0.01f;
+#else
+// -- 有地磁 --
+const float Kp = 2.0f;
+const float Ki = 0.005f;
+const float Q_dt = 0.01f;
+#endif
+
+// --- 状态变量 ---
+// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。
+static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
+// 陀螺仪积分误差项，用于补偿静态漂移
+static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
+
+// 磁力计校准相关的变量
+float         Error_Mag_f[3] = {0.0f, 0.0f, 0.0f};
+double        Sum_Avg_Mag_f[3] = {0.0, 0.0, 0.0}; // 使用double避免累加过程中的精度损失
+// 临时存储校准后数据的数组
+signed short  Temp_AccGyro[6] = {0};
+float         Temp_Mag[3] = {0.0f, 0.0f, 0.0f};
+// =================================================================================================
+
+
+
+
+/**
+* @brief 姿态角解算函数 (基于四元数和Mahony滤波器)
+* @details
+* 该函数主要完成两项工作：
+* 1.  静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
+* 2.  姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
+*     能避免万向节死锁问题。
+*
+* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
+* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
+* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]，单位为度。
+* @param mag_data_input 传入：指向包含三轴磁力计数据的结构体指针。数据单位应为高斯(Gauss)。已经8面校准过的数据
+* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将整个姿态滤波器状态重置。
+* @param quaternion_output 传出, 四元数,用于后续重力分量的去除计算
+* @return
+* - 0: 正在进行静态校准。
+* - 1: 姿态角计算成功。
+* - 2: 校准未完成，无法进行计算。
+*/
+unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t* _mag_data_input, unsigned char yaw_rst, float *quaternion_output)
+{
+#if 0  //有地磁置1
+    unsigned char sl_i = 0;
+    // 如果外部强制禁用校准，则将标志位置1
+    if (calibration_en == 0) {
+        SL_SC7U22_Error_Flag = 1;
+    }
+
+    // ====================== 坐标对齐 ======================
+    mmc5603nj_mag_data_t mag_data_input; 
+
+    mag_data_input.x = - _mag_data_input->x;
+    mag_data_input.y = - _mag_data_input->y;
+    mag_data_input.z =  _mag_data_input->z;
+    // ================================================================
+
+
+    // =================================================================================
+    //   静态校准 
+    // ---------------------------------------------------------------------------------
+    if (SL_SC7U22_Error_Flag == 0) {
+        unsigned short acc_gyro_delta[2];
+        acc_gyro_delta[0] = 0;
+        acc_gyro_delta[1] = 0;
+        for (sl_i = 0; sl_i < 3; sl_i++) {
+            acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
+            acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
+        }
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
+        }
+#if   (ACC_RANGE == 2)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) {
+#elif (ACC_RANGE == 4)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
+#elif (ACC_RANGE == 8)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) {
+#elif (ACC_RANGE == 16)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) {
+#endif
+            if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
+        } else {
+            SL_SC7U22_Error_cnt = 0;
+        }
+        if (SL_SC7U22_Error_cnt > 190) {
+            //累加6轴数据
+            for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
+
+            Sum_Avg_Mag_f[0] += mag_data_input.x;
+            Sum_Avg_Mag_f[1] += mag_data_input.y;
+            Sum_Avg_Mag_f[2] += mag_data_input.z;
+
+            SL_SC7U22_Error_cnt2++;
+            if (SL_SC7U22_Error_cnt2 > 49) {
+                SL_SC7U22_Error_Flag = 1;
+                SL_SC7U22_Error_cnt2 = 0;
+                SL_SC7U22_Error_cnt = 0;
+                //6轴偏置
+                for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
+                Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
+                Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
+                #if ACC_RANGE==2
+                Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==4
+                Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==8
+                Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==16
+                Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
+                #endif
+                Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
+                Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
+                Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
+                // //磁力计偏置 -- 不在这弄,在磁力计初始化的时候开始
+                // Sum_Avg_Mag_f[0] /= 50.0;
+                // Sum_Avg_Mag_f[1] /= 50.0;
+                // Sum_Avg_Mag_f[2] /= 50.0;
+                // Error_Mag_f[0] = 0.0f - (float)Sum_Avg_Mag_f[0];
+                // Error_Mag_f[1] = 0.0f - (float)Sum_Avg_Mag_f[1];
+                // Error_Mag_f[2] = 0.0f - (float)Sum_Avg_Mag_f[2];
+            }
+        } else {
+            SL_SC7U22_Error_cnt2 = 0;
+            for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
+            // Sum_Avg_Mag_f[0] = 0.0;
+            // Sum_Avg_Mag_f[1] = 0.0;
+            // Sum_Avg_Mag_f[2] = 0.0;
+        }
+        return 0; // 返回0，表示正在校准
+    }
+    
+    // =================================================================================
+    //  姿态解算 (Mahony AHRS)
+    // ---------------------------------------------------------------------------------
+    if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
+
+        // --- Yaw轴/姿态重置 ---
+        // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态
+        if (yaw_rst == 1) {
+            q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
+            exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f;
+        }
+
+        // --- 数据预处理 ---
+        // 应用零点偏移补偿
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
+        }
+
+        // Temp_Mag[0] = mag_data_input.x + Error_Mag_f[0];
+        // Temp_Mag[1] = mag_data_input.y + Error_Mag_f[1];
+        // Temp_Mag[2] = mag_data_input.z + Error_Mag_f[2];
+        Temp_Mag[0] = mag_data_input.x;
+        Temp_Mag[1] = mag_data_input.y;
+        Temp_Mag[2] = mag_data_input.z;
+
+        // 将校准后的数据写回输入数组
+#if 1 
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
+        }
+#endif
+        
+        // 获取校准后的数据
+        float ax = (float)Temp_Accgyro[0];
+        float ay = (float)Temp_Accgyro[1];
+        float az = (float)Temp_Accgyro[2];
+        // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s)
+        // 转换系数 0.061 ≈ 2000dps / 32768 LSB;  PI/180 ≈ 0.01745
+        float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate
+        float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate
+        float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate
+        float mx = Temp_Mag[0];
+        float my = Temp_Mag[1];
+        float mz = Temp_Mag[2];
+
+        // --- Mahony算法核心 ---
+        float norm;
+        float q0q0 = q0 * q0;
+        float q0q1 = q0 * q1;
+        float q0q2 = q0 * q2;
+        float q0q3 = q0 * q3;
+        float q1q1 = q1 * q1;
+        float q1q2 = q1 * q2;
+        float q1q3 = q1 * q3;
+        float q2q2 = q2 * q2;
+        float q2q3 = q2 * q3;
+        float q3q3 = q3 * q3;
+        float hx, hy, bx, bz;
+        float vx, vy, vz, wx, wy, wz;
+        float ex, ey, ez;
+
+        // 归一化加速度计测量值，得到单位重力向量
+        norm = sqrtf(ax * ax + ay * ay + az * az);
+        if (norm > 0.0f) { ax /= norm; ay /= norm; az /= norm; }
+        else { return 1; }
+        
+        norm = sqrtf(mx * mx + my * my + mz * mz);
+        if (norm > 0.0f) { mx /= norm; my /= norm; mz /= norm; }
+
+        // 根据当前姿态(四元数)估计重力向量的方向
+        vx = 2.0f * (q1q3 - q0q2);
+        vy = 2.0f * (q0q1 + q2q3);
+        vz = q0q0 - q1q1 - q2q2 + q3q3;
+
+        // 计算磁场误差 (倾斜补偿)
+        hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
+        hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
+        bx = sqrtf(hx * hx + hy * hy);
+        bz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
+        wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
+        wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
+        wz = 2.0f * bx * (q1q3 + q0q2) + 2.0f * bz * (0.5f - q1q1 - q2q2);
+
+        // 合并重力和磁场误差
+        ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+        ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+        ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+
+        // PI控制器
+        if (Ki > 0.0f) {
+            exInt += ex * Ki * Q_dt;
+            eyInt += ey * Ki * Q_dt;
+            ezInt += ez * Ki * Q_dt;
+        }
+        gx += Kp * ex + exInt;
+        gy += Kp * ey + eyInt;
+        gz += Kp * ez + ezInt;
+
+        // 使用校正后的角速度更新四元数 (一阶毕卡法)
+        q0 += (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt;
+        q1 += ( q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt;
+        q2 += ( q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt;
+        q3 += ( q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt;
+
+        // 归一化四元数，保持其单位长度
+        norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+        q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
+
+        // --- 将四元数转换为欧拉角 (Pitch, Roll, Yaw) ---
+        // Pitch (绕Y轴旋转)
+        Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
+        // Roll (绕X轴旋转)
+        Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3) * 57.29578f;
+        // Yaw (绕Z轴旋转) 
+        Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.29578f;
+
+        //将四元数传出去
+        if (quaternion_output != NULL) {
+			quaternion_output[0] = q0; // w
+			quaternion_output[1] = q1; // x
+			quaternion_output[2] = q2; // y
+			quaternion_output[3] = q3; // z
+		}
+        return 1; // 返回1，表示计算成功
+    }
+
+    return 2; // 校准未完成，返回错误状态
+
+#else
+    unsigned char sl_i = 0;
+    
+    // 如果外部强制禁用校准，则将标志位置1
+    if (calibration_en == 0) {
+        SL_SC7U22_Error_Flag = 1;
+    }
+
+    // =================================================================================
+    // 步骤 1: 静态校准 (此部分逻辑与原代码完全相同)
+    // ---------------------------------------------------------------------------------
+    if (SL_SC7U22_Error_Flag == 0) {
+        unsigned short acc_gyro_delta[2];
+        acc_gyro_delta[0] = 0;
+        acc_gyro_delta[1] = 0;
+        for (sl_i = 0; sl_i < 3; sl_i++) {
+            acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
+            acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
+        }
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
+        }
+#if   (ACC_RANGE == 2)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) {
+#elif (ACC_RANGE == 4)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
+#elif (ACC_RANGE == 8)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) {
+#elif (ACC_RANGE == 16)
+        if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) {
+#endif
+            if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
+        } else {
+            // printf("error: The calibration process has undergone a shift.\n");
+            SL_SC7U22_Error_cnt = 0;
+        }
+        if (SL_SC7U22_Error_cnt > 190) {
+            for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
+            SL_SC7U22_Error_cnt2++;
+            if (SL_SC7U22_Error_cnt2 > 49) {
+                SL_SC7U22_Error_Flag = 1;
+                SL_SC7U22_Error_cnt2 = 0;
+                SL_SC7U22_Error_cnt = 0;
+                for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
+
+
+                Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
+                Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
+                #if ACC_RANGE==2
+                Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==4
+                Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==8
+                Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
+                #elif ACC_RANGE==16
+                Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
+                #endif
+                Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
+                Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
+                Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
+
+                
+                // xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
+                // xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
+            }
+        } else {
+            SL_SC7U22_Error_cnt2 = 0;
+            for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
+        }
+        return 0; // 返回0，表示正在校准
+    }
+    
+    // =================================================================================
+    // 步骤 2: 姿态解算 (Mahony AHRS)
+    // ---------------------------------------------------------------------------------
+    if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
+
+        // --- 2.1 Yaw轴/姿态重置 ---
+        // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态
+        if (yaw_rst == 1) {
+            q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
+            exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f;
+        }
+
+        // --- 2.2 数据预处理 ---
+        // 应用零点偏移补偿
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
+        }
+
+        // 将校准后的数据写回输入数组 (可选)
+#if 1 
+        for (sl_i = 0; sl_i < 6; sl_i++) {
+            acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
+        }
+#endif
+        
+        // 获取校准后的数据
+        float ax = (float)Temp_Accgyro[0];
+        float ay = (float)Temp_Accgyro[1];
+        float az = (float)Temp_Accgyro[2];
+        // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s)
+        // 转换系数 0.061 ≈ 2000dps / 32768 LSB;  PI/180 ≈ 0.01745
+        float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate
+        float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate
+        float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate
+
+        // --- 2.3 Mahony算法核心 ---
+        float norm;
+        float vx, vy, vz; // 估计的重力向量
+        float ex, ey, ez; // 加速度计测量和估计的重力向量之间的误差
+
+        // 归一化加速度计测量值，得到单位重力向量
+        norm = sqrtf(ax * ax + ay * ay + az * az);
+        if (norm > 0.0f) { // 防止除以零
+            ax = ax / norm;
+            ay = ay / norm;
+            az = az / norm;
+
+            // 根据当前姿态(四元数)估计重力向量的方向
+            vx = 2.0f * (q1 * q3 - q0 * q2);
+            vy = 2.0f * (q0 * q1 + q2 * q3);
+            vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+            // 计算测量值与估计值之间的误差（叉积）
+            ex = (ay * vz - az * vy);
+            ey = (az * vx - ax * vz);
+            ez = (ax * vy - ay * vx);
+
+            // 积分误差项，用于消除陀螺仪的静态漂移
+            exInt = exInt + ex * Ki * Q_dt;
+            eyInt = eyInt + ey * Ki * Q_dt;
+            ezInt = ezInt + ez * Ki * Q_dt;
+
+            float kp_dynamic = Kp; // 默认使用全局Kp
+
+            // // 计算重力向量与Z轴的夹角余弦值
+            // // 当设备接近水平时，abs_az_component 接近 1
+            // // 当设备接近垂直时，abs_az_component 接近 0
+            // float abs_az_component = fabsf(az); 
+
+            // // 设置一个阈值，比如当与水平面的夹角大于75度时 (cos(75) approx 0.26)
+            // // 就开始降低Kp
+            // if (abs_az_component < 0.26f) {
+            //     // 线性降低Kp，或者直接使用一个较小的值
+            //     // 越接近垂直，az越小，Kp也越小
+            //     kp_dynamic = Kp * (abs_az_component / 0.26f); 
+            // }
+
+            // 使用PI控制器校正陀螺仪的测量值
+            gx += kp_dynamic * ex + exInt;
+            gy += kp_dynamic * ey + eyInt;
+            gz += kp_dynamic * ez + ezInt;
+
+            
+            // gx = gx + Kp * ex + exInt;
+            // gy = gy + Kp * ey + eyInt;
+            // gz = gz + Kp * ez + ezInt;
+        }
+
+        // 使用校正后的角速度更新四元数 (一阶毕卡法)
+        q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt;
+        q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt;
+        q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt;
+        q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt;
+
+        // 归一化四元数，保持其单位长度
+        norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+        q0 = q0 / norm;
+        q1 = q1 / norm;
+        q2 = q2 / norm;
+        q3 = q3 / norm;
+
+        // --- 2.4 将四元数转换为欧拉角 (Pitch, Roll, Yaw) ---
+        // 转换公式将四元数转换为 ZYX 顺序的欧拉角
+        // 结果单位为弧度，乘以 57.29578f 转换为度
+        
+        // Pitch (绕Y轴旋转)
+        Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
+        
+        // Roll (绕X轴旋转)
+        Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * 57.29578f;
+        
+        // Yaw (绕Z轴旋转) - 注意：无磁力计的6轴IMU，Yaw角会随时间漂移
+        Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f;
+
+
+		if (quaternion_output != NULL) {
+			quaternion_output[0] = q0; // w
+			quaternion_output[1] = q1; // x
+			quaternion_output[2] = q2; // y
+			quaternion_output[3] = q3; // z
+		}
+
+        return 1; // 返回1，表示计算成功
+    }
+
+    return 2; // 校准未完成，返回错误状态
+#endif
+}
 
 #endif
 
+///////////////////////////////////////////////////////////////////////////////////////////////////
 
-/*
+#if 0
-//-----------------------------------------
+// --- 新增：定义自动化校准状态 ---
-调用示例-by_lmx：
+typedef enum {
+    CAL_STATE_IDLE,             // 0: 空闲，未开始校准
+    CAL_STATE_WAIT_STILL,       // 1: 等待设备静止
+    CAL_STATE_COLLECTING,       // 2: 正在采集数据
+    CAL_STATE_PAUSE_BETWEEN_FACES, // 3: 面与面之间的暂停
+    CAL_STATE_CALCULATING,      // 4: 正在计算最终参数
+    CAL_STATE_FINISHED,         // 5: 校准完成，参数有效
+} SL_AutoCalib_State;
+
+// --- 新增：定义校准的面 ---
+typedef enum {
+    CAL_FACE_POS_Z = 0, // Z+
+    CAL_FACE_NEG_Z,     // Z-
+    CAL_FACE_POS_Y,     // Y+
+    CAL_FACE_NEG_Y,     // Y-
+    CAL_FACE_POS_X,     // X+
+    CAL_FACE_NEG_X,     // X-
+    CAL_FACE_COUNT      // 总面数
+} SL_Calib_Face;
+
+// --- 校准参数常量 ---
+#define CAL_STILL_TIME_THRESHOLD 190 // 需要静止的采样周期数 (约1.9s)
+#define CAL_SAMPLES_TO_COLLECT 50    // 每个面采集的样本数
+#define CAL_PAUSE_SECONDS        4   // 面间暂停秒数
+
+// --- 修改/新增 全局变量 ---
+static SL_AutoCalib_State g_calib_state = CAL_STATE_IDLE;
+static SL_Calib_Face g_current_calib_face = CAL_FACE_POS_Z;
+static unsigned short g_still_count = 0;
+static unsigned char  g_samples_collected = 0;
+static unsigned short g_pause_timer = 0;
+
+static long g_calib_data_sum[6] = {0};
+static signed short g_calib_avg_data[CAL_FACE_COUNT][6] = {{0}};
+
+static float g_acc_offset[3] = {0.0f};
+static float g_acc_scale[3] = {1.0f, 1.0f, 1.0f};
+static signed short g_gyro_offset[3] = {0}; // 陀螺仪偏移用整数更符合原始数据类型
+
+static unsigned char g_calibration_valid = 0;
 
 
-//1.
-// 定义用于存放传感器数据的缓冲区
-static signed short acc_raw_data[3];    // [0]: acc_x, [1]: acc_y, [2]: acc_z
-static signed short gyr_raw_data[3];    // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
-static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
-static float final_angle_data[3];      // [0]: Pitch, [1]: Roll, [2]: Yaw
 
-// 传感器数据处理任务
+#if (ACC_RANGE == 2)
-void sensor_processing_task(void *priv)
+#define G_VALUE (16384.0f)
-{
+#elif (ACC_RANGE == 4)
-    while (1) {
+#define G_VALUE (8192.0f)
-        // 4. 周期性调用读取函数，获取原始数据
+#elif (ACC_RANGE == 8)
-        SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
+#define G_VALUE (4096.0f)
-
+#elif (ACC_RANGE == 16)
-        // 5. 合并加速度和角速度数据到一个数组中
+#define G_VALUE (2048.0f)
-        // SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
+#endif
-        memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
+/**
-        memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
+ * @brief IMU姿态解算函数，增加了六面校准功能。
-
+ * @param calibration_cmd   校准命令，来自 SL_Calibration_Cmd 枚举。
-        // 6. 调用姿态解算函数
+ * @param acc_gyro_input    指向6轴原始数据的指针 (AccX,Y,Z, GyroX,Y,Z)。
-        // 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
+ * @param Angle_output      指向3轴姿态角输出的指针 (Pitch, Roll, Yaw)。
-        // calibration_en = 1: 让函数内部自动管理校准过程
+ * @param yaw_rst           Yaw轴复位标志。
-        // yaw_rst = 0:      不复位Yaw角
+ * @return unsigned char    0: 正在校准, 1: 计算成功, 2: 校准未完成，无法计算。
-        unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
-
-        // 7. 检查函数返回的状态
-        if (status == 1) {
-            // 计算成功，final_angle_data 中的数据有效
-            xlog("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
-        } else if (status == 0) {
-            // 传感器正在进行静态校准，此时角度数据可能不准确
-            xlog("Sensor is calibrating...\n");
-        } else {
-            // status == 2, 表示校准未完成或发生错误
-            xlog("Angle calculation error or calibration not finished.\n");
-        }
-
-        // 延时一段时间，例如10ms (对应100Hz)
-        os_time_dly(1);
-    }
-}
-
-// 应用程序主入口或初始化函数
-void app_main()
-{
-    // ... 其他初始化代码 ...
-
-    // 2. 调用初始化函数来配置SCU722传感器
-    unsigned char init_success = SL_SC7U22_Config();
-
-    if (init_success) {
-        xlog("SCU722 初始化成功!\n");
-        // 3. 创建一个任务来周期性地读取和处理数据
-        task_create(sensor_processing_task, NULL, "sensor_task");
-    } else {
-        xlog("SCU722 初始化失败!\n");
-    }
-
-    // ...
-}
-
-//-----------------------------------------
  */
+unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
+{
+    unsigned char sl_i = 0;
+
+    // 启动自动化校准流程
+    if (auto_calib_start == 1 && g_calib_state == CAL_STATE_IDLE) {
+        g_calib_state = CAL_STATE_WAIT_STILL;
+        g_current_calib_face = CAL_FACE_POS_Z;
+        g_calibration_valid = 0;
+        xlog("\n\n===== Auto-Calibration Started =====\r\n");
+        xlog("Step 1/%d: Place device with Z-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT);
+    }
+
+    // =================================================================================
+    // 步骤 1: 自动化校准状态机
+    // ---------------------------------------------------------------------------------
+    if (g_calib_state != CAL_STATE_IDLE && g_calib_state != CAL_STATE_FINISHED) {
+        // --- 静止检测 (通用逻辑) ---
+        unsigned short acc_delta = 0, gyro_delta = 0;
+        for (sl_i = 0; sl_i < 3; sl_i++) {
+            acc_delta += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
+            gyro_delta += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
+        }
+        for (sl_i = 0; sl_i < 6; sl_i++) Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
+        
+        int is_still = (acc_delta < 160) && (gyro_delta < 40);
+
+        switch (g_calib_state) {
+            case CAL_STATE_WAIT_STILL:
+                if (is_still) {
+                    if (++g_still_count >= CAL_STILL_TIME_THRESHOLD) {
+                        g_calib_state = CAL_STATE_COLLECTING;
+                        g_samples_collected = 0;
+                        g_still_count = 0;
+                        for(sl_i = 0; sl_i < 6; sl_i++) g_calib_data_sum[sl_i] = 0;
+                        xlog("Device is still. Collecting data...\r\n");
+                    }
+                } else {
+                    g_still_count = 0; // 如果移动则重置计数
+                }
+                break;
+
+            case CAL_STATE_COLLECTING:
+                if (!is_still) { // 如果在采集中移动了，则重新开始等待静止
+                    g_calib_state = CAL_STATE_WAIT_STILL;
+                    g_still_count = 0;
+                    xlog("Movement detected! Please keep the device still.\r\n");
+                    break;
+                }
+                
+                if (g_samples_collected < CAL_SAMPLES_TO_COLLECT) {
+                    for (sl_i = 0; sl_i < 6; sl_i++) {
+                        g_calib_data_sum[sl_i] += acc_gyro_input[sl_i];
+                    }
+                    g_samples_collected++;
+                } else {
+                    // 当前面采集完成
+                    for (sl_i = 0; sl_i < 6; sl_i++) {
+                        g_calib_avg_data[g_current_calib_face][sl_i] = g_calib_data_sum[sl_i] / CAL_SAMPLES_TO_COLLECT;
+                    }
+                    xlog("Face %d data collected.\r\n", g_current_calib_face + 1);
+                    
+                    if (g_current_calib_face < CAL_FACE_NEG_X) {
+                        g_calib_state = CAL_STATE_PAUSE_BETWEEN_FACES;
+                        g_pause_timer = 0;
+                        xlog("Pausing for %d seconds. Please prepare for the next orientation.\r\n", CAL_PAUSE_SECONDS);
+                    } else {
+                        // 所有面都已完成
+                        g_calib_state = CAL_STATE_CALCULATING;
+                    }
+                }
+                break;
+
+            case CAL_STATE_PAUSE_BETWEEN_FACES:
+                if (++g_pause_timer >= (CAL_PAUSE_SECONDS * 1000 / dt)) { 
+                    g_current_calib_face++;
+                    g_calib_state = CAL_STATE_WAIT_STILL;
+                    g_still_count = 0;
+                    switch(g_current_calib_face) {
+                        case CAL_FACE_NEG_Z: xlog("Step 2/%d: Place device with Z-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
+                        case CAL_FACE_POS_Y: xlog("Step 3/%d: Place device with Y-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
+                        case CAL_FACE_NEG_Y: xlog("Step 4/%d: Place device with Y-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
+                        case CAL_FACE_POS_X: xlog("Step 5/%d: Place device with X-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
+                        case CAL_FACE_NEG_X: xlog("Step 6/%d: Place device with X-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
+                    }
+                }
+                break;
+
+            case CAL_STATE_CALCULATING:
+                xlog("All data collected. Calculating calibration parameters...\r\n");
+                // 计算陀螺仪偏移
+                long gyro_sum[3] = {0};
+                for (sl_i = 0; sl_i < CAL_FACE_COUNT; sl_i++) {
+                    gyro_sum[0] += g_calib_avg_data[sl_i][3]; // Gx
+                    gyro_sum[1] += g_calib_avg_data[sl_i][4]; // Gy
+                    gyro_sum[2] += g_calib_avg_data[sl_i][5]; // Gz
+                }
+                g_gyro_offset[0] = gyro_sum[0] / CAL_FACE_COUNT;
+                g_gyro_offset[1] = gyro_sum[1] / CAL_FACE_COUNT;
+                g_gyro_offset[2] = gyro_sum[2] / CAL_FACE_COUNT;
+
+                // 计算加速度计偏移和增益
+                g_acc_offset[0] = (g_calib_avg_data[CAL_FACE_POS_X][0] + g_calib_avg_data[CAL_FACE_NEG_X][0]) / 2.0f;
+                g_acc_scale[0]  = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_X][0] - g_calib_avg_data[CAL_FACE_NEG_X][0]);
+                g_acc_offset[1] = (g_calib_avg_data[CAL_FACE_POS_Y][1] + g_calib_avg_data[CAL_FACE_NEG_Y][1]) / 2.0f;
+                g_acc_scale[1]  = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Y][1] - g_calib_avg_data[CAL_FACE_NEG_Y][1]);
+                g_acc_offset[2] = (g_calib_avg_data[CAL_FACE_POS_Z][2] + g_calib_avg_data[CAL_FACE_NEG_Z][2]) / 2.0f;
+                g_acc_scale[2]  = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Z][2] - g_calib_avg_data[CAL_FACE_NEG_Z][2]);
+                
+                g_calibration_valid = 1;
+                g_calib_state = CAL_STATE_FINISHED;
+                xlog("===== Calibration Finished! =====\r\n");
+                xlog("Acc Offset: %.2f, %.2f, %.2f\r\n", g_acc_offset[0], g_acc_offset[1], g_acc_offset[2]);
+                xlog("Acc Scale:  %.4f, %.4f, %.4f\r\n", g_acc_scale[0], g_acc_scale[1], g_acc_scale[2]);
+                xlog("Gyro Offset: %d, %d, %d\r\n", g_gyro_offset[0], g_gyro_offset[1], g_gyro_offset[2]);
+                break;
+            default:
+                break;
+        }
+        return 0; // 只要还在校准流程中，就返回0
+    }
+    
+
+    // =================================================================================
+    // 步骤 2: 姿态解算
+    // ---------------------------------------------------------------------------------
+    if (g_calibration_valid == 0) {
+        return 2; // 校准未完成，无法进行姿态解算
+    }
+
+    float angle_acc[3] = {0};
+    float gyro_val[3] = {0};
+    float calibrated_acc[3] = {0};
+
+    // --- 2.1 数据预处理 (应用新的校准参数) ---
+    // 应用偏移和增益校准加速度计
+    calibrated_acc[0] = ((float)acc_gyro_input[0] - g_acc_offset[0]) * g_acc_scale[0];
+    calibrated_acc[1] = ((float)acc_gyro_input[1] - g_acc_offset[1]) * g_acc_scale[1];
+    calibrated_acc[2] = ((float)acc_gyro_input[2] - g_acc_offset[2]) * g_acc_scale[2];
+
+    // 应用偏移校准陀螺仪
+    gyro_val[0] = ((float)acc_gyro_input[4] - g_gyro_offset[1]) * 0.061; // GYR-Y -> Pitch
+    gyro_val[1] = ((float)acc_gyro_input[3] - g_gyro_offset[0]) * 0.061; // GYR-X -> Roll
+    gyro_val[2] = ((float)acc_gyro_input[5] - g_gyro_offset[2]) * 0.061; // GYR-Z -> Yaw
+    
+    // --- 2.2 使用校准后的加速度计计算姿态角 ---
+    // 将校准后的加速度值转换为归一化的重力分量
+    angle_acc[0] = calibrated_acc[0] / G_VALUE; // ax
+    angle_acc[1] = calibrated_acc[1] / G_VALUE; // ay
+    angle_acc[2] = calibrated_acc[2] / G_VALUE; // az
+
+    // 限制范围，防止asinf/atanf计算错误
+    if (angle_acc[0] > 1.0f)  angle_acc[0] = 1.0f;
+    if (angle_acc[0] < -1.0f) angle_acc[0] = -1.0f;
+    // ... (对 angle_acc[1] 和 angle_acc[2] 也做同样处理)
+
+    angle_acc[0] = asinf(angle_acc[0]) * 57.29578f; // Pitch
+    angle_acc[1] = atan2f(angle_acc[1], angle_acc[2]) * 57.29578f; // Roll (使用atan2更稳健)
+
+    // =================================================================================
+	// 步骤 2.4: 卡尔曼滤波
+	// 对Pitch和Roll分别进行滤波
+	// ---------------------------------------------------------------------------------
+
+	/************** Pitch 轴滤波 **************/
+	// --- 预测步骤 ---
+	// 1. 预测状态：根据上一时刻的角度和当前角速度，预测当前角度
+	angle0[0] += (gyro_val[0] - q_bias0[0]) * dt;
+	// 2. 预测协方差：更新P矩阵，表示预测状态的不确定性
+	Pdot0[0] = Q_angle - P0[0][1] - P0[1][0] + P0[1][1] * dt;
+	Pdot0[1] = -P0[1][1];
+	Pdot0[2] = -P0[1][1];
+	Pdot0[3] = Q_gyro;
+	P0[0][0] += Pdot0[0] * dt;
+	P0[0][1] += Pdot0[1] * dt;
+	P0[1][0] += Pdot0[2] * dt;
+	P0[1][1] += Pdot0[3] * dt;
+
+	// --- 更新步骤 ---
+	// 1. 计算卡尔曼增益 K
+	PCt0_0[0] = C_0 * P0[0][0];
+	PCt0_1[0] = C_0 * P0[1][0];
+	E0[0] = R_angle + C_0 * PCt0_0[0];
+	if (E0[0] == 0) { E0[0] = 0.0001; } // 防止除零
+	K0_0[0] = PCt0_0[0] / E0[0];
+	K0_1[0] = PCt0_1[0] / E0[0];
+
+	// 2. 计算测量余差（innovation）
+	angle_err0[0] = angle_acc[0] - angle0[0];
+	// 3. 更新状态估计：结合预测值和测量值，得到最优估计
+	angle0[0] += K0_0[0] * angle_err0[0];
+	// 4. 更新陀螺仪偏置估计
+	q_bias0[0] += K0_1[0] * angle_err0[0];
+	angle_dot0[0] = gyro_val[0] - q_bias0[0];
+
+	// 5. 更新协方差矩阵 P
+	t0_0[0] = PCt0_0[0];
+	t0_1[0] = C_0 * P0[0][1];
+	P0[0][0] -= K0_0[0] * t0_0[0];
+	P0[0][1] -= K0_0[0] * t0_1[0];
+	P0[1][0] -= K0_1[0] * t0_0[0];
+	P0[1][1] -= K0_1[0] * t0_1[0];
+
+	// 输出最终的Pitch角
+	Angle_output[0] = angle0[0];
+
+	/************** Roll 轴滤波 (过程同Pitch) **************/
+	// --- 预测步骤 ---
+	angle0[1] += (gyro_val[1] - q_bias0[1]) * dt;
+	Pdot1[0] = Q_angle - P1[0][1] - P1[1][0] + P1[1][1] * dt;
+	Pdot1[1] = -P1[1][1];
+	Pdot1[2] = -P1[1][1];
+	Pdot1[3] = Q_gyro;
+	P1[0][0] += Pdot1[0] * dt;
+	P1[0][1] += Pdot1[1] * dt;
+	P1[1][0] += Pdot1[2] * dt;
+	P1[1][1] += Pdot1[3] * dt;
+
+	// --- 更新步骤 ---
+	PCt0_0[1] = C_1 * P1[0][0];
+	PCt0_1[1] = C_1 * P1[1][0];
+	E0[1] = R_angle + C_1 * PCt0_0[1];
+	if (E0[1] == 0) { E0[1] = 0.0001; }
+	K0_0[1] = PCt0_0[1] / E0[1];
+	K0_1[1] = PCt0_1[1] / E0[1];
+	angle_err0[1] = angle_acc[1] - angle0[1];
+	angle0[1] += K0_0[1] * angle_err0[1];
+	q_bias0[1] += K0_1[1] * angle_err0[1];
+	angle_dot0[1] = gyro_val[1] - q_bias0[1];
+	t0_0[1] = PCt0_0[1];
+	t0_1[1] = C_1 * P1[0][1];
+	P1[0][0] -= K0_0[1] * t0_0[1];
+	P1[0][1] -= K0_0[1] * t0_1[1];
+	P1[1][0] -= K0_1[1] * t0_0[1];
+	P1[1][1] -= K0_1[1] * t0_1[1];
+
+	// 输出最终的Roll角
+	Angle_output[1] = angle0[1];
+
+	/************** Yaw 轴计算 **************/
+	// Yaw角无法通过加速度计（重力）来校正，因此这里只使用陀螺仪进行简单积分。
+	// 这种方法会因为陀螺仪的漂移而导致误差随时间累积。
+	if (yaw_rst == 1) {
+		Angle_output[2] = 0; // 如果有复位信号，则清零
+	}
+
+	// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
+	if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
+		Angle_output[2] += gyro_val[2] * dt;
+	}
+
+    return 1; // 返回1，表示计算成功
+}
+#endif

apps/earphone/xtell_Sensor/sensor/SC7U22.h

@@ -9,6 +9,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved.
 
 #include "gSensor/gSensor_manage.h"
 #include "printf.h"
+#include "MMC56.h"
 
 //是否使能串口打印调试
 #define SL_Sensor_Algo_Release_Enable  0x00
@@ -19,12 +20,12 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved.
 /***使用前请根据实际情况配置以下参数******/
 /**SC7U22的SDO 接地:  0****************/
 /**SC7U22的SDO 接电源:1****************/
-#define SL_SC7U22_SDO_VDD_GND            1
+#define SL_SC7U22_SDO_VDD_GND            0
 /*****************************************/
 /***使用前请根据实际IIC地址配置参数***/
 /**SC7U22的IIC 接口地址为 7bits:  0****/
 /**SC7U22的IIC 接口地址为 8bits:  1****/
-#define SL_SC7U22_IIC_7BITS_8BITS        0
+#define SL_SC7U22_IIC_7BITS_8BITS        1
 /*****************************************/
 #if SL_SC7U22_SDO_VDD_GND==0
 #define SL_SC7U22_IIC_7BITS_ADDR        0x18
@@ -130,8 +131,10 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
 /**output Angle_output[2]:   Yaw*******************************/
 /**input  yaw_rst:   reset yaw value***************************/
 
+void set_SC7U22_Error_Flag(char flag);
 unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
-
+unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
+unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t *mag_data_input, unsigned char yaw_rst, float *quaternion_output);
 unsigned char get_calibration_state(void);
 /**寄存器宏定义*******************************/
 #define SC7U22_WHO_AM_I         0x01

apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c

@@ -925,72 +925,3 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
 }
 #endif
 
-
-/*
-//-----------------------------------------
-调用示例-by_lmx：
-
-
-//1.
-// 定义用于存放传感器数据的缓冲区
-static signed short acc_raw_data[3];    // [0]: acc_x, [1]: acc_y, [2]: acc_z
-static signed short gyr_raw_data[3];    // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
-static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
-static float final_angle_data[3];      // [0]: Pitch, [1]: Roll, [2]: Yaw
-
-// 传感器数据处理任务
-void sensor_processing_task(void *priv)
-{
-    while (1) {
-        // 4. 周期性调用读取函数，获取原始数据
-        SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
-
-        // 5. 合并加速度和角速度数据到一个数组中
-        // SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
-        memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
-        memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
-
-        // 6. 调用姿态解算函数
-        // 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
-        // calibration_en = 1: 让函数内部自动管理校准过程
-        // yaw_rst = 0:      不复位Yaw角
-        unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
-
-        // 7. 检查函数返回的状态
-        if (status == 1) {
-            // 计算成功，final_angle_data 中的数据有效
-            printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
-        } else if (status == 0) {
-            // 传感器正在进行静态校准，此时角度数据可能不准确
-            printf("Sensor is calibrating...\n");
-        } else {
-            // status == 2, 表示校准未完成或发生错误
-            printf("Angle calculation error or calibration not finished.\n");
-        }
-
-        // 延时一段时间，例如10ms (对应100Hz)
-        os_time_dly(1);
-    }
-}
-
-// 应用程序主入口或初始化函数
-void app_main()
-{
-    // ... 其他初始化代码 ...
-
-    // 2. 调用初始化函数来配置SCU722传感器
-    unsigned char init_success = SL_SC7U22_Config();
-
-    if (init_success) {
-        printf("SCU722 初始化成功!\n");
-        // 3. 创建一个任务来周期性地读取和处理数据
-        task_create(sensor_processing_task, NULL, "sensor_task");
-    } else {
-        printf("SCU722 初始化失败!\n");
-    }
-
-    // ...
-}
-
-//-----------------------------------------
-*/

apps/earphone/xtell_Sensor/sensor/WF282A.c

@@ -0,0 +1,181 @@
+/*
+    气压计 - WF282A
+*/
+#include "wf282a.h"
+#include <math.h>
+#include <stdint.h> // 推荐使用标准类型
+#include "gSensor/gSensor_manage.h"
+
+/*==================================================================================*/
+/*    WF282A 内部定义                                                               */
+/*==================================================================================*/
+
+// 存储校准系数的静态全局变量
+static int16_t c0, c1, c01, c11, c20, c21, c30;
+static int32_t c00, c10;
+
+/*==================================================================================*/
+/*     封装的底层I2C读写函数                                                         */
+/*==================================================================================*/
+
+/**
+ * @brief 写入单个字节到WF282A寄存器
+ */
+static void wf282a_write_reg(uint8_t reg, uint8_t data) {
+    gravity_sensor_command(WF_IIC_WRITE_ADDRESS, reg, data);
+}
+
+/**
+ * @brief 从WF282A读取多个字节
+ */
+static uint32_t wf282a_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
+    return _gravity_sensor_get_ndata(WF_IIC_READ_ADDRESS, reg, buf, len);
+}
+
+/*==================================================================================*/
+/*    内部辅助函数                                                                  */
+/*==================================================================================*/
+
+/**
+ * @brief 从缓冲区中解析所有校准系数
+ * @param buf 包含从寄存器0x10开始读取的18个字节的校准数据
+ */
+static void parse_calibration_data(const uint8_t *buf) {
+    // c0 (12-bit)
+    c0 = ((int16_t)buf[0] << 4) | (buf[1] >> 4);
+    if (c0 & (1 << 11)) c0 |= 0xF000;
+
+    // c1 (12-bit)
+    c1 = (((int16_t)buf[1] & 0x0F) << 8) | buf[2];
+    if (c1 & (1 << 11)) c1 |= 0xF000;
+
+    // c00 (20-bit)
+    c00 = ((int32_t)buf[3] << 12) | ((int32_t)buf[4] << 4) | (buf[5] >> 4);
+    if (c00 & (1 << 19)) c00 |= 0xFFF00000;
+
+    // c10 (20-bit)
+    c10 = (((int32_t)buf[5] & 0x0F) << 16) | ((int32_t)buf[6] << 8) | buf[7];
+    if (c10 & (1 << 19)) c10 |= 0xFFF00000;
+
+    // c01, c11, c20, c21, c30 (16-bit)
+    c01 = (int16_t)((uint16_t)buf[8] << 8 | buf[9]);
+    c11 = (int16_t)((uint16_t)buf[10] << 8 | buf[11]);
+    c20 = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);
+    c21 = (int16_t)((uint16_t)buf[14] << 8 | buf[15]);
+    c30 = (int16_t)((uint16_t)buf[16] << 8 | buf[17]);
+}
+
+/**
+ * @brief 获取原始温度值 (ADC)
+ */
+static int32_t Get_Traw() {
+    uint8_t buff[3];
+    int32_t Traw;
+    // 从 MSB 寄存器 WF_TMP_B2 (0x03) 开始连续读取3个字节
+    wf282a_read_regs(WF_TMP_B2, buff, 3);
+    // buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
+    Traw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
+    // 24位二进制补码转32位
+    if (Traw & (1 << 23)) {
+        Traw |= 0xFF000000;
+    }
+    return Traw;
+}
+
+/**
+ * @brief 获取原始气压值 (ADC)
+ */
+static int32_t Get_Praw() {
+    uint8_t buff[3];
+    int32_t Praw;
+    // 从 MSB 寄存器 WF_PRS_B2 (0x00) 开始连续读取3个字节
+    wf282a_read_regs(WF_PRS_B2, buff, 3);
+    // buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
+    Praw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
+    // 24位二进制补码转32位
+    if (Praw & (1 << 23)) {
+        Praw |= 0xFF000000;
+    }
+    return Praw;
+}
+
+/*==================================================================================*/
+/* 4. 外部接口函数实现                                                              */
+/*==================================================================================*/
+
+uint8_t WF_Init() {
+    uint8_t calib_buf[18];
+    uint8_t check_cfg;
+
+    // 1. 配置传感器工作模式
+    // 推荐配置：压力8次过采样，温度1次过采样，测量速率16Hz
+    wf282a_write_reg(WF_PRS_CFG, (PM_RATE_16 << 4) | PM_PRC_8);
+    wf282a_write_reg(WF_TMP_CFG, (TMP_RATE_16 << 4) | TMP_PRC_1 | TMP_INT_SENSOR);
+    wf282a_write_reg(WF_MEAS_CFG, 0x07); // 启动连续压力和温度测量
+    wf282a_write_reg(WF_CFG_REG, 0x00);  // 无中断或FIFO移位配置
+
+    // 2. 一次性读取所有校准系数 (从0x10到0x21，共18字节)
+    if (wf282a_read_regs(COEF_C0, calib_buf, 18) != 0) {
+        return 2; // 读取校准数据失败
+    }
+    parse_calibration_data(calib_buf);
+
+    // 3. 检查配置是否写入成功
+    wf282a_read_regs(WF_MEAS_CFG, &check_cfg, 1);
+    if (check_cfg != 0x07) {
+        return 1; // 错误
+    } else {
+        return 0; // 成功
+    }
+}
+
+void WF_Sleep() {
+    wf282a_write_reg(WF_MEAS_CFG, 0x00); // 待机模式
+}
+
+void WF_Wakeup() {
+    wf282a_write_reg(WF_MEAS_CFG, 0x07); // 恢复连续测量
+}
+
+uint8_t WF_GetID() {
+    uint8_t id;
+    wf282a_read_regs(WF_ID_REG, &id, 1);
+    return id;
+}
+
+float WF_Temperature_Calculate() {
+    float Traw_sc;
+    int32_t Traw = Get_Traw();
+
+    Traw_sc = (float)Traw / KT; // 缩放原始温度值
+    return (float)c0 * 0.5f + (float)c1 * Traw_sc;
+}
+
+float WF_Pressure_Calculate() {
+    float Traw_sc, Praw_sc, Pcomp;
+    int32_t Traw = Get_Traw();
+    int32_t Praw = Get_Praw();
+
+    Traw_sc = (float)Traw / KT; // 缩放原始温度值
+    Praw_sc = (float)Praw / KP; // 缩放原始压力值
+
+    // 公式: 手册给出
+    Pcomp = (float)c00
+            + Praw_sc * ((float)c10 + Praw_sc * ((float)c20 + Praw_sc * (float)c30))
+            + Traw_sc * (float)c01
+            + Traw_sc * Praw_sc * ((float)c11 + Praw_sc * (float)c21);
+
+    return Pcomp;
+}
+
+float WF_Altitude_Calculate() {
+    float pressure_pa = WF_Pressure_Calculate();
+    // 使用标准大气压公式计算海拔
+    // P = P0 * (1 - L*h / T0)^(g*M / (R*L))
+    // 简化公式: h = 44330 * (1 - (P/P0)^(1/5.255))
+    // 1/5.255 ≈ 0.1903
+    if (pressure_pa <= 0) {
+        return 0.0f; // 避免无效计算
+    }
+    return 44330.0f * (1.0f - powf(pressure_pa / 101325.0f, 0.1902949f));
+}

apps/earphone/xtell_Sensor/sensor/WF282A.h

@@ -0,0 +1,148 @@
+#ifndef _WF282A_H_
+#define _WF282A_H_
+
+#include <stdint.h> // 使用标准整数类型
+
+// 标定值
+#define KT                          524288.0f
+#define KP                          1572864.0f
+
+
+#define WF_PULL_UP 1  //外部是否接的上拉
+
+// I2C 从设备地址
+#if WF_PULL_UP == 1   //外部接的高
+#define WF_IIC_7BIT_ADDRESS 0x77 //7位,外部接高为0x77
+#define WF_IIC_WRITE_ADDRESS  (WF_IIC_7BIT_ADDRESS<<1)  //8位地址
+#define WF_IIC_READ_ADDRESS   (WF_IIC_WRITE_ADDRESS | 0x01)
+#else
+#define WF_IIC_7BIT_ADDRESS 0x76  //7位,外部接低为0x76
+#define WF_IIC_WRITE_ADDRESS  (WF_IIC_7BIT_ADDRESS<<1)  //8位地址
+#define WF_IIC_READ_ADDRESS   (WF_IIC_WRITE_ADDRESS | 0x01)
+#endif
+
+#define WF_CHIP_ID                  0X10
+
+// 寄存器映射
+// 压力数据
+#define WF_PRS_B2                   0x00
+#define WF_PRS_B1                   0x01
+#define WF_PRS_B0                   0x02
+// 温度数据
+#define WF_TMP_B2                   0x03
+#define WF_TMP_B1                   0x04
+#define WF_TMP_B0                   0x05
+// 配置寄存器
+#define WF_PRS_CFG                  0x06
+#define WF_TMP_CFG                  0x07
+#define WF_MEAS_CFG                 0x08
+#define WF_CFG_REG                  0x09
+#define WF_INT_STS                  0x0A
+#define WF_FIFO_STS                 0x0B
+#define WF_RESET_REG                0x0C
+// ID寄存器
+#define WF_ID_REG                   0x0D
+// 校准系数寄存器
+#define COEF_C0                     0x10
+#define COEF_C0_C1                  0x11
+#define COEF_C1                     0x12
+#define COEF_C00_H                  0x13
+#define COEF_C00_L                  0x14
+#define COEF_C00_C10                0x15
+#define COEF_C10_M                  0x16
+#define COEF_C10_L                  0x17
+#define COEF_C01_H                  0x18
+#define COEF_C01_L                  0x19
+#define COEF_C11_H                  0x1A
+#define COEF_C11_L                  0x1B
+#define COEF_C20_H                  0x1C
+#define COEF_C20_L                  0x1D
+#define COEF_C21_H                  0x1E
+#define COEF_C21_L                  0x1F
+#define COEF_C30_H                  0x20
+#define COEF_C30_L                  0x21
+
+// --- 配置宏  ---
+
+// 压力配置 (PRS_CFG[6:4]) - 测量速率
+#define PM_RATE_1                   0x00 // 1   次/秒
+#define PM_RATE_2                   0x01 // 2   次/秒
+#define PM_RATE_4                   0x02 // 4   次/秒
+#define PM_RATE_8                   0x03 // 8   次/秒
+#define PM_RATE_16                  0x04 // 16  次/秒
+#define PM_RATE_32                  0x05 // 32  次/秒
+#define PM_RATE_64                  0x06 // 64  次/秒
+#define PM_RATE_128                 0x07 // 128 次/秒
+// 压力配置 (PRS_CFG[3:0]) - 过采样率
+#define PM_PRC_1                    0x00 // 1   次 (单次)
+#define PM_PRC_2                    0x01 // 2   次 (低功耗)
+#define PM_PRC_4                    0x02 // 4   次
+#define PM_PRC_8                    0x03 // 8   次 (标准)
+#define PM_PRC_16                   0x04 // 16  次 (需要移位)
+#define PM_PRC_32                   0x05 // 32  次 (需要移位)
+#define PM_PRC_64                   0x06 // 64  次 (高精度, 需要移位)
+#define PM_PRC_128                  0x07 // 128 次 (需要移位)
+
+// 温度配置 (TMP_CFG[7]) - 传感器源
+#define TMP_EXT_SENSOR              0x80 // 使用外部传感器
+#define TMP_INT_SENSOR              0x00 // 使用内部传感器
+// 温度配置 (TMP_CFG[6:4]) - 测量速率
+#define TMP_RATE_1                  0x00 // 1   次/秒
+#define TMP_RATE_2                  0x01 // 2   次/秒
+#define TMP_RATE_4                  0x02 // 4   次/秒
+#define TMP_RATE_8                  0x03 // 8   次/秒
+#define TMP_RATE_16                 0x04 // 16  次/秒
+#define TMP_RATE_32                 0x05 // 32  次/秒
+#define TMP_RATE_64                 0x06 // 64  次/秒
+#define TMP_RATE_128                0x07 // 128 次/秒
+// 温度配置 (TMP_CFG[3:0]) - 过采样率
+#define TMP_PRC_1                   0x00 // 1   次
+#define TMP_PRC_2                   0x01 // 2   次
+#define TMP_PRC_4                   0x02 // 4   次
+#define TMP_PRC_8                   0x03 // 8   次
+#define TMP_PRC_16                  0x04 // 16  次
+#define TMP_PRC_32                  0x05 // 32  次
+#define TMP_PRC_64                  0x06 // 64  次
+#define TMP_PRC_128                 0x07 // 128 次
+
+/**
+ * @brief 初始化WF282A传感器
+ * @return 0: 成功, 1: 失败
+ */
+uint8_t WF_Init(void);
+
+/**
+ * @brief 使传感器进入休眠/待机模式
+ */
+void WF_Sleep(void);
+
+/**
+ * @brief 唤醒传感器，开始连续测量
+ */
+void WF_Wakeup(void);
+
+/**
+ * @brief 获取传感器芯片ID
+ * @return 芯片ID (应为 0x10)
+ */
+uint8_t WF_GetID(void);
+
+/**
+ * @brief 计算并返回当前海拔高度
+ * @return 海拔高度 (单位: 米)
+ */
+float WF_Altitude_Calculate(void);
+
+/**
+ * @brief 计算并返回补偿后的压力值
+ * @return 压力 (单位: Pa)
+ */
+float WF_Pressure_Calculate(void);
+
+/**
+ * @brief 计算并返回补偿后的温度值
+ * @return 温度 (单位: °C)
+ */
+float WF_Temperature_Calculate(void);
+
+#endif // _WF282A_H_

apps/earphone/xtell_Sensor/xtell.h

@@ -1,5 +1,6 @@
 #ifndef XTELL_H
 #define XTELL_H
+#include "system/includes.h"
 
 // #define KS_BLE 1
 #define XTELL_TEST 1

apps/earphone/xtell_Sensor/xtell_handler.c

@@ -45,6 +45,11 @@
 #include "default_event_handler.h"
 #include "debug.h"
 #include "system/event.h"
+#include "./ano/ano_protocol.h"
+#include "./sensor/MMC56.h"
+#include "./sensor/BMP280.h"
+#include "./sensor/AK8963.h"
+#include "./calculate/skiing_tracker.h"
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 //宏定义
 #define LOG_TAG_CONST       EARPHONE
@@ -80,13 +85,28 @@ unsigned char xtell_bl_state=0;   //存放经典蓝牙的连接状态，0断开
 u8 bt_newname =0;
 unsigned char xt_ble_new_name[9] = "CM-22222";
 static u16 play_poweron_ok_timer_id = 0;
+
+// -- 初始化标志位 --
+u8 SC7U22_init = 0x10;     //六轴是否初始化
+u8 MMC5603nj_init = 0x20;  //地磁是否初始化
+u8 BMP280_init = 0x30;   //气压计初始化
+u8 foot_init = 0x40;  //数据来源初始化：左脚0x41 or 右脚0x42
+// -- 线程id --
+
+
+u16 gsensor_test_id = 0;
 //
 ///////////////////////////////////////////////////////////////////////////////////////////////////
-
-
-
 extern int bt_hci_event_handler(struct bt_event *bt);
-
+extern void SC7U22_static_calibration(void);
+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);
+extern void start_collect_fuc(void);
+extern void BLE_send_fuc(void);
+extern void start_calibration(void);
+extern void start_clloct(void);
+extern void stop_clloct(void);
+extern void set_foot_state(u8 state);
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 /*
  * 模式状态机, 通过start_app()控制状态切换
@@ -199,27 +219,89 @@ void le_user_app_event_handler(struct sys_event* event){
                 if(event->u.app.buffer[2] == 0x01){      //后面的数据长度 1
                     switch (event->u.app.buffer[3]){
                         case 0x01:
-                            extern void start_detection(void);
+                            extern void gsensor_test(void);
-                            start_detection();
+                            create_process(&gsensor_test_id,"gsensor_test",NULL,gsensor_test,1000);
-                            char* send_tmp = "start_detection\n";
-                            send_data_to_ble_client(send_tmp,strlen(send_tmp));
                             break;
-                        case 0x02:
+                        case 0xff:  //测试
-                            extern void stop_detection(void);
+                            u8 device_buff[10];
-                            stop_detection();
+                            u8 founds = 0;
-                            send_tmp = "stop_detection\n";
+                            extern void i2c_scanner_probe(u8* device_addr, u8* found_number);
-                            send_data_to_ble_client(send_tmp,strlen(send_tmp));
+                            i2c_scanner_probe(device_buff,&founds);
-                            break;
+                            for(int i = 0;i < founds;i++){
-                        case 0x03:
+                                send_data_to_ble_client(&device_buff,founds);
-                            extern void clear_speed(void);
+                            }
-                            clear_speed();
-                            send_tmp = "Reset speed and distances to zero\n";
-                            send_data_to_ble_client(send_tmp,strlen(send_tmp));
-                            
                             break;
                         default:
                             break;
-                        
+                    }
+                }else if(event->u.app.buffer[2] == 0x02){  //后面数据长度为2
+                    switch (event->u.app.buffer[3]){  //数据包类型
+                        case 0x00: //数据包类型为：指定传感器初始化
+                            u8 send2_0[5] = {0xBB,0xBE,0x02,0x00,0x00};
+                            if(event->u.app.buffer[4] == 0x01){  //六轴
+                                if (SL_SC7U22_Config() == 0) {
+                                    send2_0[4] = 0x00;    //初始化失败
+                                    SC7U22_init = 0x10;
+                                    send_data_to_ble_client(&send2_0,5);
+                                    return;
+                                }
+                                start_calibration();
+                            }else if(event->u.app.buffer[4] == 0x02){  //地磁
+                                if(mmc5603nj_init() == 0){
+                                    MMC5603nj_init = 0x20;
+                                    send2_0[4] = MMC5603nj_init;  //地磁初始化失败
+                                    send_data_to_ble_client(&send2_0,5);
+                                    return;
+                                }
+                                MMC5603nj_init = 0x21;
+                                send2_0[4] = MMC5603nj_init;  //地磁初始化成功
+                                send_data_to_ble_client(&send2_0,5);
+                            }else if(event->u.app.buffer[4] == 0x03){  //气压计初始化
+                                if(bmp280_init() != 0){
+                                    //初始化失败
+                                    BMP280_init = 0x30;
+                                    send2_0[4] = BMP280_init;
+                                    send_data_to_ble_client(&send2_0,5);
+                                    return;
+                                }
+                                BMP280_init = 0x31;
+                                send2_0[4] = BMP280_init;  //气压计初始化成功
+                                send_data_to_ble_client(&send2_0,5);
+                            }
+                            break;
+                        case 0x01: //设置传感器采集对象：左脚or右脚
+                            u8 send2_1[9] = {0xBB,0xBE,0x06,0x05,0x00,0x00,0x00,0x00,0x00};
+                            if(event->u.app.buffer[4] == 0x01){ //设定数据来源是左脚
+                                foot_init = 0x41; 
+                            }else if(event->u.app.buffer[4] == 0x02){//设定数据来源是右脚
+                                foot_init = 0x42; 
+                            }
+                            send2_1[4] = foot_init;
+                            send_data_to_ble_client(&send2_1,9);
+                            break;
+                        case 0x02: //数据包类型为：获取指定传感器初始化状态
+                            u8 send2_2[5] = {0xBB,0xBE,0x02,0x00,0x00};
+                            if(event->u.app.buffer[4] == 0x01){  //六轴
+                                send2_2[4] = SC7U22_init;
+                            }else if(event->u.app.buffer[4] == 0x02){  //地磁
+                                send2_2[4] = MMC5603nj_init;
+                            }else if(event->u.app.buffer[4] == 0x03){  //气压计
+                                send2_2[4] = BMP280_init;
+                            }
+                            send_data_to_ble_client(&send2_2,5);
+                            break;
+                        case 0x03:  //开始/停止滑雪计算 
+                            if(event->u.app.buffer[4] == 0x01){  //开始滑雪计算
+                                if(SC7U22_init == 0x10 || MMC5603nj_init == 0x20 || BMP280_init == 0x30){ //传感器未进行初始化
+                                    u8 send2_3[5] = {0xBB,0xBE,0x02,0x00,0x00};
+                                    send_data_to_ble_client(&send2_3,5);
+                                    return;
+                                }
+                                start_clloct();
+                            }else if(event->u.app.buffer[4] == 0x02){ //停止滑雪计算
+                                stop_clloct();
+                            }
+                            break;
                     }
                 }
             }

cpu/br28/iic_hw.c

@@ -188,12 +188,18 @@ void hw_iic_stop(hw_iic_dev iic)
 
 u8 hw_iic_tx_byte(hw_iic_dev iic, u8 byte)
 {
+    // printf("====debug1=======\n");
     u8 id = iic_get_id(iic);
+    // printf("====debug2=======\n");
     iic_dir_out(iic_regs[id]);
+    // printf("====debug3=======\n");
     iic_buf_reg(iic_regs[id]) = byte;
+    // printf("====debug4=======\n");
     iic_cfg_done(iic_regs[id]);
+    // printf("====debug5=======\n");
     /* putchar('a'); */
     while (!iic_pnd(iic_regs[id]));
+    // printf("====debug6=======\n");
     iic_pnd_clr(iic_regs[id]);
     /* putchar('b'); */
     return iic_send_is_ack(iic_regs[id]);

cpu/br28/sdk.ld

@@ -253,6 +253,7 @@
   _MASK_MEM_BEGIN = ABSOLUTE(0x19fc00);
   _MASK_MEM_SIZE = ABSOLUTE(0x1a4);
 
+
 EXTERN(
 _start
 
@@ -274,25 +275,14 @@ cvsd_decoder
 
 
 pcm_decoder
-
-
-
-
-
-
-
-mp3_decoder
 wtgv2_decoder
-
-
-
-aac_decoder
 cvsd_encoder
 
 
 
 msbc_encoder
 audio_dac_driver
+
 );
 
 UPDATA_SIZE = 0x80;
@@ -428,6 +418,7 @@ SECTIONS
         battery_notify_begin = .;
         *(.battery_notify)
         battery_notify_end = .;
+
   . = ALIGN(4);
   __VERSION_BEGIN = .;
         KEEP(*(.sys.version))
@@ -518,6 +509,7 @@ SECTIONS
  *(.audio_track_data)
  *(.audio_adc_data)
 
+
   . = ALIGN(4);
         *(.data*)
 
@@ -726,6 +718,7 @@ SECTIONS
 
     } > ram0
 
+
  data_code_pc_limit_end = .;
  __report_overlay_end = .;
 
@@ -815,6 +808,7 @@ SECTIONS
 }
 
 
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -846,6 +840,7 @@ SECTIONS
 
     UPDATE_CODE_TOTAL_SIZE = update_code_end - update_code_start;
 }
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -948,6 +943,7 @@ BTSTACK_LE_HOST_MESH_RAM_TOTAL = BTSTACK_LE_HOST_MESH_DATA_SIZE + BTSTACK_LE_HOS
 BTSTACK_LE_HOST_MESH_FLASH_TOTAL = BTSTACK_LE_HOST_MESH_CODE_SIZE;
 
 BTSTACK_CODE_SIZE = (btstack_code_end - btstack_code_start) + (btstack_data_end - btstack_data_start);
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -1241,6 +1237,7 @@ SECTIONS
   *(.os_code)
     } > ram0
 }
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -1450,6 +1447,7 @@ SECTIONS
     BTCTLER_RAM_TOTAL = (btctler_data_end - btctler_data_start) + (btctler_bss_end - btctler_bss_start);
     BTCTLER_CODE_TOTAL = (btctler_code_end - btctler_code_start);
 }
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -1567,6 +1565,7 @@ SECTIONS
         *(.timer.text.cache.L1)
         *(.gpio.text.cache.L1)
         *(.iic_hw.text.cache.L1)
+
         driver_data_code_end = .;
   . = ALIGN(4);
     } > ram0
@@ -1577,6 +1576,7 @@ SECTIONS
     DRIVER_DATA_CODE_TOTAL = (driver_data_code_end - driver_data_code_start);
 
 }
+
 SECTIONS
 {
     .data : ALIGN(4)
@@ -2016,6 +2016,7 @@ SECTIONS
  } > ram0
 }
 
+
 text_begin = ADDR(.text);
 text_size = SIZEOF(.text);
 text_end = text_begin + text_size;

cpu/br28/sdk_used_list.used

@@ -1,54 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-sdfile_vfs_ops
-sbc_decoder
-
-
-
-msbc_decoder
-
-
-
-sbc_hwaccel
-
-
-
-cvsd_decoder
-
-
-
-pcm_decoder
-
-
-
-
-
-
-
-mp3_decoder
-wtgv2_decoder
-
-
-
-aac_decoder
-cvsd_encoder
-
-
-
-msbc_encoder
-audio_dac_driver

cpu/br28/tools/download/earphone/646-AC690X-7603.key

@@ -1 +0,0 @@
-64F3350FE2590FAF79755623B7E159CE83FAD97C34014E5EB2B528F6D6C2DABAB3B7C88C

cpu/br28/tools/download/earphone/AC69.key

@@ -1 +0,0 @@
-1cfdc466ec927dbedd4d12447b6bbff1da1bd6ddb6b375ecda1bd6ddb6b375ec950b1206

cpu/br28/tools/download/earphone/AC690X-8029.key

@@ -1 +0,0 @@
-39209e0c95b837ce6cd626f6b1d566a6a5fed6fa11cd451ba5fed6fa11cd451b9a3f2514

cpu/br28/tools/download/earphone/download.bat

@@ -1,49 +0,0 @@
-@echo off
-
-cd %~dp0
-
-copy ..\..\script.ver .
-copy ..\..\tone.cfg .
-copy ..\..\p11_code.bin .
-copy ..\..\br28loader.bin .
-copy ..\..\ota.bin .
-copy ..\..\anc_coeff.bin .
-copy ..\..\anc_gains.bin .
-
-..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key AC69.key -format all -key 646-AC690X-7603.key
-
-@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
-
-:: -format all
-::-reboot 2500
-
-@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
-if exist *.mp3 del *.mp3 
-if exist *.PIX del *.PIX
-if exist *.TAB del *.TAB
-if exist *.res del *.res
-if exist *.sty del *.sty
-
-..\..\ufw_maker.exe -fw_to_ufw jl_isd.fw
-copy jl_isd.ufw update.ufw
-del jl_isd.ufw
-
-@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
-::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
-
-::IF EXIST jl_696x.bin del jl_696x.bin 
-
-if exist script.ver del script.ver
-if exist tone.cfg del tone.cfg
-if exist p11_code.bin del p11_code.bin
-if exist br28loader.bin del br28loader.bin
-if exist anc_coeff.bin del anc_coeff.bin
-if exist anc_gains.bin del anc_gains.bin
-
-@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
-@rem -format vm        //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
-@rem -format cfg       //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
-@rem -format 0x3f0-2   //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
-
-ping /n 2 127.1>null
-IF EXIST null del null

cpu/br28/tools/download/earphone/download_app_ota.bat

@@ -1,49 +0,0 @@
-@echo off
-
-cd %~dp0
-
-copy ..\..\script.ver .
-copy ..\..\tone.cfg .
-copy ..\..\p11_code.bin .
-copy ..\..\br28loader.bin .
-copy ..\..\ota.bin .
-copy ..\..\anc_coeff.bin .
-copy ..\..\anc_gains.bin .
-
-..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key 646-AC690X-7603.key
-
-@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
-
-:: -format all
-::-reboot 2500
-
-@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
-if exist *.mp3 del *.mp3 
-if exist *.PIX del *.PIX
-if exist *.TAB del *.TAB
-if exist *.res del *.res
-if exist *.sty del *.sty
-
-..\..\ufw_maker.exe -fw_to_ufw jl_isd.fw
-copy jl_isd.ufw update.ufw
-del jl_isd.ufw
-
-@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
-::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
-
-::IF EXIST jl_696x.bin del jl_696x.bin 
-
-if exist script.ver del script.ver
-if exist tone.cfg del tone.cfg
-if exist p11_code.bin del p11_code.bin
-if exist br28loader.bin del br28loader.bin
-if exist anc_coeff.bin del anc_coeff.bin
-if exist anc_gains.bin del anc_gains.bin
-
-@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
-@rem -format vm        //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
-@rem -format cfg       //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
-@rem -format 0x3f0-2   //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
-
-ping /n 2 127.1>null
-IF EXIST null del null

cpu/br28/tools/download/earphone/download_no_key.bat

@@ -1,49 +0,0 @@
-@echo off
-
-cd %~dp0
-
-copy ..\..\script.ver .
-copy ..\..\tone.cfg .
-copy ..\..\p11_code.bin .
-copy ..\..\br28loader.bin .
-copy ..\..\ota.bin .
-copy ..\..\anc_coeff.bin .
-copy ..\..\anc_gains.bin .
-
-:: -format all -key AC690X-8029.key
-..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress
-@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
-
-:: -format all
-::-reboot 2500
-
-@rem ɾ    ʱ ļ -format all
-if exist *.mp3 del *.mp3 
-if exist *.PIX del *.PIX
-if exist *.TAB del *.TAB
-if exist *.res del *.res
-if exist *.sty del *.sty
-
-..\..\ufw_maker.exe -fw_to_ufw jl_isd.fw
-copy jl_isd.ufw update.ufw
-del jl_isd.ufw
-
-@REM          ļ      ļ 
-::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
-
-::IF EXIST jl_696x.bin del jl_696x.bin 
-
-if exist script.ver del script.ver
-if exist tone.cfg del tone.cfg
-if exist p11_code.bin del p11_code.bin
-if exist br28loader.bin del br28loader.bin
-if exist anc_coeff.bin del anc_coeff.bin
-if exist anc_gains.bin del anc_gains.bin
-
-@rem         ˵  
-@rem -format vm        //    VM     
-@rem -format cfg       //    BT CFG     
-@rem -format 0x3f0-2   //  ʾ ӵ  0x3f0    sector   ʼ         2    sector(  һ      Ϊ16   ƻ 10   ƶ  ɣ  ڶ             10    )
-
-ping /n 2 127.1>null
-IF EXIST null del null

cpu/br28/tools/download/earphone/download_old_key.bat

@@ -1,50 +0,0 @@
-@echo off
-
-cd %~dp0
-
-copy ..\..\script.ver .
-copy ..\..\tone.cfg .
-copy ..\..\p11_code.bin .
-copy ..\..\br28loader.bin .
-copy ..\..\ota.bin .
-copy ..\..\anc_coeff.bin .
-copy ..\..\anc_gains.bin .
-
-:: -format all -key AC690X-8029.key
-..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key AC690X-8029.key
-
-@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
-
-:: -format all
-::-reboot 2500
-
-@rem ɾ    ʱ ļ -format all
-if exist *.mp3 del *.mp3 
-if exist *.PIX del *.PIX
-if exist *.TAB del *.TAB
-if exist *.res del *.res
-if exist *.sty del *.sty
-
-..\..\ufw_maker.exe -fw_to_ufw jl_isd.fw
-copy jl_isd.ufw update.ufw
-del jl_isd.ufw
-
-@REM          ļ      ļ 
-::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
-
-::IF EXIST jl_696x.bin del jl_696x.bin 
-
-if exist script.ver del script.ver
-if exist tone.cfg del tone.cfg
-if exist p11_code.bin del p11_code.bin
-if exist br28loader.bin del br28loader.bin
-if exist anc_coeff.bin del anc_coeff.bin
-if exist anc_gains.bin del anc_gains.bin
-
-@rem         ˵  
-@rem -format vm        //    VM     
-@rem -format cfg       //    BT CFG     
-@rem -format 0x3f0-2   //  ʾ ӵ  0x3f0    sector   ʼ         2    sector(  һ      Ϊ16   ƻ 10   ƶ  ɣ  ڶ             10    )
-
-ping /n 2 127.1>null
-IF EXIST null del null

cpu/br28/tools/download/soundbox/download.bat

@@ -1,47 +0,0 @@
-@echo off
-
-cd %~dp0
-
-copy ..\..\script.ver .
-copy ..\..\tone.cfg .
-copy ..\..\p11_code.bin .
-copy ..\..\br28loader.bin .
-
-..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res tone.cfg ..\..\cfg_tool.bin ..\..\eq_cfg_hw.bin p11_code.bin -uboot_compress
-
-:: -format all
-::-reboot 2500
-
-@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
-if exist *.mp3 del *.mp3 
-if exist *.PIX del *.PIX
-if exist *.TAB del *.TAB
-if exist *.res del *.res
-if exist *.sty del *.sty
-
-@rem <20><><EFBFBD>ɹ̼<C9B9><CCBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
-..\..\fw_add.exe -noenc -fw jl_isd.fw -add ..\..\ota.bin -type 100 -out jl_isd.fw
-@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ýű<C3BD><C5B1>İ汾<C4B0><E6B1BE>Ϣ<EFBFBD><CFA2> FW <20>ļ<EFBFBD><C4BC><EFBFBD>
-..\..\fw_add.exe -noenc -fw jl_isd.fw -add script.ver -out jl_isd.fw
-
-..\..\ufw_maker.exe -fw_to_ufw jl_isd.fw
-copy jl_isd.ufw update.ufw
-del jl_isd.ufw
-
-@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
-::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
-
-::IF EXIST jl_696x.bin del jl_696x.bin 
-
-if exist script.ver del script.ver
-if exist tone.cfg del tone.cfg
-if exist p11_code.bin del p11_code.bin
-if exist br28loader.bin del br28loader.bin
-
-@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
-@rem -format vm        //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
-@rem -format cfg       //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
-@rem -format 0x3f0-2   //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
-
-ping /n 2 127.1>null
-IF EXIST null del null

cpu/br28/tools/isd_config.ini

@@ -1,82 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-[EXTRA_CFG_PARAM]
-NEW_FLASH_FS = YES;
-
-
-
-
-
-
-CHIP_NAME = AC701N;
-ENTRY = 0x6000100;
-PID = AC701N;
-VID = 0.01;
-
-SDK_VERSION = JL701N_V1.6.1;
-
-RESERVED_OPT = 0;
-
-UFW_ELEMENT = 0x10 - 0x0, 0x1 - 0x0;
-
-DOWNLOAD_MODEL = USB;
-SERIAL_DEVICE_NAME = JlVirtualJtagSerial;
-SERIAL_BARD_RATE = 1000000;
-SERIAL_CMD_OPT = 2;
-SERIAL_CMD_RATE = 100;
-SERIAL_CMD_RES = 0;
-SERIAL_INIT_BAUD_RATE = 9600;
-LOADER_BAUD_RATE = 1000000;
-LOADER_ASK_BAUD_RATE = 1000000;
-BEFORE_LOADER_WAIT_TIME = 150;
-SERIAL_SEND_KEY = YES;
-
-
-
-
-
-
-[CHIP_VERSION]
-SUPPORTED_LIST = P, B;
-
-[SYS_CFG_PARAM]
-SPI = 2_3_0_0;
-
-
-
-
-
-
-UTTX = PB02;
-UTBD = 1000000;
-UTRX = PP00;
-[FW_ADDITIONAL]
-FILE_LIST = (file = ota.bin: type = 100);
-[RESERVED_CONFIG]
-BTIF_ADR = AUTO;
-BTIF_LEN = 0x1000;
-BTIF_OPT = 1;
-PRCT_ADR = 0;
-PRCT_LEN = CODE_LEN;
-PRCT_OPT = 2;
-
-
-
-VM_ADR = 0;
-VM_LEN = 8K;
-VM_OPT = 1;
-[BURNER_CONFIG]
-SIZE = 32;