四元数求角度和去重力分量，误差减少

apps/earphone/xtell_Sensor/calculate/skiing_tracker.c

@@ -9,6 +9,16 @@
 #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)阈值
 // 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
@@ -168,7 +178,7 @@ void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, f
 }
 
 /**
- * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
+ * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
  * @details 同样，此方法比使用欧拉角更优。
  * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
  * @param q                 输入：表示姿态的四元数 [w, x, y, z]
@@ -188,7 +198,7 @@ void q_transform_to_world_with_quaternion(const float *acc_linear_device, const
     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;
+    // acc_linear_world[2] -= G_ACCELERATION;
 }
 
 
@@ -451,8 +461,15 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
             break;
         case WHEEL:
         case NO_CONSTANT_SPEED:
-            q_transform_to_world_with_quaternion(acc_device_ms2, quaternion_data, tracker->acc_no_g);
+            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 轴
@@ -512,6 +529,37 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
             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, 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
+
+
 }