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

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 ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务
@@ -63,3 +64,32 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
 - 重复定位的数据？
 - 静态时的角度误差？
 
+
+
+定时器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/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
+
+
 }
 
 

apps/earphone/xtell_Sensor/send_data.c

@@ -179,7 +179,7 @@ typedef struct {
     signed short acc_data[3];
     signed short gyr_data[3];
     float angle[3];  //pitch roll yaw
-    float quaternion_output[4];
+    float quaternion_output[4];  //四元数数据
 } sensor_data_t;
 static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
 
@@ -213,10 +213,12 @@ void sensor_read_data(){
     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 = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
         // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
         // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
 
+        status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
+
         int count = 0;
         if(count > 100){
             count = 0;
@@ -233,9 +235,11 @@ void sensor_read_data(){
         }
     } else {
         // printf("Calculate the time interval =============== start\n");
-        status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
+        // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
         // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
         // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
+        status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
+
         memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
         memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
         if(mutex1 == 0){
@@ -329,7 +333,7 @@ void BLE_send_data(){
         // send_data_to_ble_client(&log_buffer,strlen(log_buffer));
 
         memset(&log_buffer, 0, 100);
-        #if 0
+        #if 1
         // 使用 snprintf 进行格式化
         num_chars_written = snprintf(
             log_buffer,                      // 目标缓冲区
@@ -557,7 +561,7 @@ void xtell_task_create(void){
 
 
     // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
-
+// 
     // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4);
 
     // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);