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.
This commit is contained in:
+18
-12
@@ -25,15 +25,19 @@
|
||||
|
||||
#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
|
||||
/* IMU坐标系 -> 机体坐标系旋转矩阵(必须为右手系旋转,行列式为+1)
|
||||
* 用户根据IMU实际安装方向填写,out = R * raw
|
||||
* 常见安装方向示例(供参考):
|
||||
* 正常安装: {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}
|
||||
* 绕Z转180°: {{-1, 0, 0}, { 0,-1, 0}, { 0, 0, 1}}
|
||||
* 绕X转180°: {{ 1, 0, 0}, { 0,-1, 0}, { 0, 0,-1}}
|
||||
* 绕Y转180°: {{-1, 0, 0}, { 0, 1, 0}, { 0, 0,-1}}
|
||||
*/
|
||||
#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轴)
|
||||
* @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]);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user