refactor: replace IMU mount enum with user-defined rotation matrix

Replace compile-time orientation enum/macro approach with a general
3x3 rotation matrix IMU2BODY_ROTATION. User fills in the matrix
according to actual IMU mounting orientation. Matrix must be a proper
rotation (det = +1) to preserve right-handed coordinate system.

modules/imu/ins_task.c

@@ -116,34 +116,20 @@ INS_t *INS_Init(void)
 }
 
 /**
- * @brief 根据IMU安装方向宏，对原始数据进行轴映射和符号修正
+ * @brief 通过旋转矩阵将IMU坐标系数据变换到机体坐标系
  *
- * @param raw  原始IMU数据（3轴）
- * @param out  修正后数据（3轴）
+ * out = IMU2BODY_ROTATION * raw
+ *
+ * @param raw  IMU坐标系原始数据（3轴）
+ * @param out  机体坐标系数据（3轴）
  */
 void IMU_Mount_Correct(const float raw[3], float out[3])
 {
-#if IMU_MOUNT_ORIENTATION == IMU_MOUNT_NORMAL
-    out[0] =  raw[0];
-    out[1] =  raw[1];
-    out[2] =  raw[2];
-#elif IMU_MOUNT_ORIENTATION == IMU_MOUNT_YAW_180
-    out[0] = -raw[0];
-    out[1] = -raw[1];
-    out[2] =  raw[2];
-#elif IMU_MOUNT_ORIENTATION == IMU_MOUNT_ROLL_180
-    out[0] =  raw[0];
-    out[1] = -raw[1];
-    out[2] = -raw[2];
-#elif IMU_MOUNT_ORIENTATION == IMU_MOUNT_PITCH_180
-    out[0] = -raw[0];
-    out[1] =  raw[1];
-    out[2] = -raw[2];
-#else
-    out[0] =  raw[0];
-    out[1] =  raw[1];
-    out[2] =  raw[2];
-#endif
+    static const float R[3][3] = IMU2BODY_ROTATION;
+
+    out[0] = R[0][0] * raw[0] + R[0][1] * raw[1] + R[0][2] * raw[2];
+    out[1] = R[1][0] * raw[0] + R[1][1] * raw[1] + R[1][2] * raw[2];
+    out[2] = R[2][0] * raw[0] + R[2][1] * raw[1] + R[2][2] * raw[2];
 }
 
 /* 注意以1kHz的频率运行此任务 */
@@ -153,14 +139,14 @@ void INS_Task(void)
     const float gravity[3] = {0, 0, 9.81f};
 
     dt = DWT_GetDeltaT(&INS_DWT_Count);
-    t += dt;
+    //t += dt;
 
     // ins update
     if ((count % 1) == 0)
     {
         BMI088_Read(&BMI088);
 
-        // IMU安装方向修正：根据宏IMU_MOUNT_ORIENTATION进行轴映射和符号翻转
+        // IMU安装方向修正：通过IMU2BODY_ROTATION旋转矩阵将IMU数据转换到机体坐标系
         {
             float imu_accel[3] = {BMI088.Accel[X], BMI088.Accel[Y], BMI088.Accel[Z]};
             float imu_gyro[3] = {BMI088.Gyro[X], BMI088.Gyro[Y], BMI088.Gyro[Z]};
@@ -211,8 +197,8 @@ void INS_Task(void)
     if ((count % 1000) == 0)
     {
         // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
-        //LOGINFO("[IMU] Temperature: %d C, PWM: %d", (int)(BMI088.Temperature * 100), (int)TempCtrl.Output);
-        //LOGINFO("[IMU] Yaw: %d, Pitch: %d, Roll: %d", (int)(INS.Yaw*100), (int)(INS.Pitch*100), (int)(INS.Roll*100));
+        LOGINFO("[IMU] Temperature: %d C, PWM: %d", (int)(BMI088.Temperature * 100), (int)TempCtrl.Output);
+        LOGINFO("[IMU] Yaw: %d, Pitch: %d, Roll: %d", (int)(INS.Yaw*100), (int)(INS.Pitch*100), (int)(INS.Roll*100));
     }
 
     count++;