cun

apps/earphone/xtell_Sensor/calculate/skiing_tracker.c

@@ -1,6 +1,4 @@
 /*
-    虽然sensor_processing_task是10ms调用一次
-    但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms
         
 */
 #include "skiing_tracker.h"
@@ -11,14 +9,14 @@
 #define G_ACCELERATION 9.81f
 #define DEG_TO_RAD (3.14159265f / 180.0f)
 
-// --- ZUPT ---
+// --- 静止检测 ---
 //两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
 // 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
-#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
+#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
 // 陀螺仪方差阈值
-#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
+#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
 // 静止时候的陀螺仪模长
-#define ZUPT_GYR_MAG_THRESHOLD  15
+#define STOP_GYR_MAG_THRESHOLD  15
 // --- --- --- 
 
 // --- 启动滑雪阈值 --- 
@@ -36,7 +34,7 @@
 // --- --- --- 
 
 // --- 原地旋转抖动 --- 
-// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
+// 用于原地旋转判断的加速度方差阈值。此值比 静止检测 阈值更宽松，
 // 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
 #define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
 // 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
@@ -68,8 +66,9 @@
 // --- 用于消除积分漂移的滤波器和阈值 ---
 // 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
 // alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
-#define HPF_ALPHA 0.995  
+#define HPF_ALPHA 0.999  
 //0.995： 0.08 Hz 的信号
+//0.999： 0.0159 Hz 
 // --- --- --- 
 
 // 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
@@ -304,7 +303,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
 
     
     // --- 静止判断 ---
-    if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD && gyr_magnitude < ZUPT_GYR_MAG_THRESHOLD) {
+    if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
         tracker->state = STATIC;
         return;
     }
@@ -639,10 +638,19 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
         printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
     }
 
+    #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==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