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轴）
+ * out = IMU2BODY_ROTATION * raw
- * @param out  修正后数据（3轴）
+ *
+ * @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
+    static const float R[3][3] = IMU2BODY_ROTATION;
-    out[0] =  raw[0];
+
-    out[1] =  raw[1];
+    out[0] = R[0][0] * raw[0] + R[0][1] * raw[1] + R[0][2] * raw[2];
-    out[2] =  raw[2];
+    out[1] = R[1][0] * raw[0] + R[1][1] * raw[1] + R[1][2] * raw[2];
-#elif IMU_MOUNT_ORIENTATION == IMU_MOUNT_YAW_180
+    out[2] = R[2][0] * raw[0] + R[2][1] * raw[1] + R[2][2] * raw[2];
-    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
 }
 
 /* 注意以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] 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] Yaw: %d, Pitch: %d, Roll: %d", (int)(INS.Yaw*100), (int)(INS.Pitch*100), (int)(INS.Roll*100));
     }
 
     count++;

modules/imu/ins_task.h

@@ -25,15 +25,19 @@
 
 #define INS_TASK_PERIOD 1
 
-/* IMU安装方向配置 */
+/* IMU坐标系 -> 机体坐标系旋转矩阵（必须为右手系旋转，行列式为+1）
-typedef enum {
+ * 用户根据IMU实际安装方向填写，out = R * raw
-    IMU_MOUNT_NORMAL = 0,   // IMU与机体坐标系一致
+ * 常见安装方向示例（供参考）：
-    IMU_MOUNT_YAW_180,      // IMU绕Z轴旋转180°
+ *   正常安装:  {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}
-    IMU_MOUNT_ROLL_180,     // IMU绕X轴旋转180°（上下颠倒）
+ *   绕Z转180°: {{-1, 0, 0}, { 0,-1, 0}, { 0, 0, 1}}
-    IMU_MOUNT_PITCH_180,    // IMU绕Y轴旋转180°（前后颠倒）
+ *   绕X转180°: {{ 1, 0, 0}, { 0,-1, 0}, { 0, 0,-1}}
-} IMU_Mount_Orientation_e;
+ *   绕Y转180°: {{-1, 0, 0}, { 0, 1, 0}, { 0, 0,-1}}
-
+ */
-#define IMU_MOUNT_ORIENTATION IMU_MOUNT_NORMAL
+#define IMU2BODY_ROTATION { \
+    { 1,  0,  0}, \
+    { 0,  -1,  0}, \
+    { 0,  0,  -1}  \
+}
 
 typedef struct
 {
@@ -78,10 +82,12 @@ typedef struct
 } IMU_Param_t;
 
 /**
- * @brief 根据IMU安装方向宏，对原始数据进行轴映射和符号修正
+ * @brief 通过变换矩阵将IMU坐标系数据变换到机体坐标系
  *
- * @param raw  原始IMU数据（3轴）
+ * out = IMU2BODY_ROTATION * raw
- * @param out  修正后数据（3轴）
+ *
+ * @param raw  IMU坐标系原始数据（3轴）
+ * @param out  机体坐标系数据（3轴）
  */
 void IMU_Mount_Correct(const float raw[3], float out[3]);