From ea4e45b940e7e5757986c053f6448a62d33034ba Mon Sep 17 00:00:00 2001
From: SkyLi <SkyLiA7Mr@gmail.com>
Date: Sun, 31 May 2026 19:45:56 +0800
Subject: [PATCH] feat: add IMU mount orientation correction via
 macro-controlled axis remapping

Support NORMAL, YAW_180, ROLL_180, PITCH_180 presets selected at compile
time by IMU_MOUNT_ORIENTATION macro. Replaces direct BMI088-to-INS
assignment with IMU_Mount_Correct() for sign flip and axis swap.
---
 modules/imu/ins_task.c | 46 +++++++++++++++++++++++++++++++++++-------
 modules/imu/ins_task.h | 18 +++++++++++++++++
 2 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c
index 9853c31..1a9a954 100644
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -115,6 +115,37 @@ INS_t *INS_Init(void)
     return &INS;
 }
 
+/**
+ * @brief 根据IMU安装方向宏，对原始数据进行轴映射和符号修正
+ *
+ * @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
+}
+
 /* 注意以1kHz的频率运行此任务 */
 void INS_Task(void)
 {
@@ -129,15 +160,16 @@ void INS_Task(void)
     {
         BMI088_Read(&BMI088);
 
-        INS.Accel[X] = BMI088.Accel[X];
-        INS.Accel[Y] = BMI088.Accel[Y];
-        INS.Accel[Z] = BMI088.Accel[Z];
-        INS.Gyro[X] = BMI088.Gyro[X];
-        INS.Gyro[Y] = BMI088.Gyro[Y];
-        INS.Gyro[Z] = BMI088.Gyro[Z];
+        // IMU安装方向修正：根据宏IMU_MOUNT_ORIENTATION进行轴映射和符号翻转
+        {
+            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]};
+            IMU_Mount_Correct(imu_accel, INS.Accel);
+            IMU_Mount_Correct(imu_gyro, INS.Gyro);
+        }
 
         // demo function,用于修正安装误差,可以不管,本demo暂时没用
-        IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
+        //IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
 
         // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
         // INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h
index dbecbe2..50687b6 100644
--- a/modules/imu/ins_task.h
+++ b/modules/imu/ins_task.h
@@ -25,6 +25,16 @@
 
 #define INS_TASK_PERIOD 1
 
+/* IMU安装方向配置 */
+typedef enum {
+    IMU_MOUNT_NORMAL = 0,   // IMU与机体坐标系一致
+    IMU_MOUNT_YAW_180,      // IMU绕Z轴旋转180°
+    IMU_MOUNT_ROLL_180,     // IMU绕X轴旋转180°（上下颠倒）
+    IMU_MOUNT_PITCH_180,    // IMU绕Y轴旋转180°（前后颠倒）
+} IMU_Mount_Orientation_e;
+
+#define IMU_MOUNT_ORIENTATION IMU_MOUNT_NORMAL
+
 typedef struct
 {
     float q[4]; // 四元数估计值
@@ -67,6 +77,14 @@ typedef struct
     float Roll;
 } IMU_Param_t;
 
+/**
+ * @brief 根据IMU安装方向宏，对原始数据进行轴映射和符号修正
+ *
+ * @param raw  原始IMU数据（3轴）
+ * @param out  修正后数据（3轴）
+ */
+void IMU_Mount_Correct(const float raw[3], float out[3]);
+
 /**
  * @brief 初始化惯导解算系统
  *