From ba3879f815f2910a23c9aef2fbaa3b8dd8a6ec5a Mon Sep 17 00:00:00 2001 From: shmily744 <1527550984@qq.com> Date: Fri, 19 Jan 2024 17:58:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=B8=8B=E4=BD=8D=E6=9C=BA?= =?UTF-8?q?=E5=8F=91=E9=80=81=E7=9A=84=E4=BA=91=E5=8F=B0=E9=99=80=E8=9E=BA?= =?UTF-8?q?=E4=BB=AA=E4=BF=A1=E6=81=AF=EF=BC=8C=E8=A7=92=E5=BA=A6=E5=88=B6?= =?UTF-8?q?=E6=94=B9=E4=B8=BA=E5=BC=A7=E5=BA=A6=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- modules/imu/ins_task.c | 74 +++++++++++++++++------------------------- 1 file changed, 29 insertions(+), 45 deletions(-) diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index ccbe26a..255912e 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -36,30 +36,26 @@ static float RefTemp = 40; // 恒温设定温度 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); -static void IMUPWMSet(uint16_t pwm) -{ - __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm); +static void IMUPWMSet(uint16_t pwm) { + __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm); } /** * @brief 温度控制 * */ -static void IMU_Temperature_Ctrl(void) -{ +static void IMU_Temperature_Ctrl(void) { PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp); IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX)); } // 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差 -static void InitQuaternion(float *init_q4) -{ +static void InitQuaternion(float *init_q4) { float acc_init[3] = {0}; float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1) float axis_rot[3] = {0}; // 旋转轴 // 读取100次加速度计数据,取平均值作为初始值 - for (uint8_t i = 0; i < 100; ++i) - { + for (uint8_t i = 0; i < 100; ++i) { BMI088_Read(&BMI088); acc_init[X] += BMI088.Accel[X]; acc_init[Y] += BMI088.Accel[Y]; @@ -78,17 +74,15 @@ static void InitQuaternion(float *init_q4) init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量) } -attitude_t *INS_Init(void) -{ +attitude_t *INS_Init(void) { if (!INS.init) INS.init = 1; else - return (attitude_t *)&INS.Gyro; + return (attitude_t *) &INS.Gyro; HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1); - while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR) - ; + while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR); IMU_Param.scale[X] = 1; IMU_Param.scale[Y] = 1; IMU_Param.scale[Z] = 1; @@ -102,23 +96,22 @@ attitude_t *INS_Init(void) IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0); // imu heat init PID_Init_Config_s config = {.MaxOut = 2000, - .IntegralLimit = 300, - .DeadBand = 0, - .Kp = 1000, - .Ki = 20, - .Kd = 0, - .Improve = 0x01}; // enable integratiaon limit + .IntegralLimit = 300, + .DeadBand = 0, + .Kp = 1000, + .Ki = 20, + .Kd = 0, + .Improve = 0x01}; // enable integratiaon limit PIDInit(&TempCtrl, &config); // noise of accel is relatively big and of high freq,thus lpf is used INS.AccelLPF = 0.0085; DWT_GetDeltaT(&INS_DWT_Count); - return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. + return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. } /* 注意以1kHz的频率运行此任务 */ -void INS_Task(void) -{ +void INS_Task(void) { static uint32_t count = 0; const float gravity[3] = {0, 0, 9.81f}; @@ -126,8 +119,7 @@ void INS_Task(void) t += dt; // ins update - if ((count % 1) == 0) - { + if ((count % 1) == 0) { BMI088_Read(&BMI088); INS.Accel[X] = BMI088.Accel[X]; @@ -159,7 +151,8 @@ void INS_Task(void) EarthFrameToBodyFrame(gravity, gravity_b, INS.q); for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波 { - INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); + INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); } BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n @@ -169,18 +162,16 @@ void INS_Task(void) INS.YawTotalAngle = QEKF_INS.YawTotalAngle; //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); - VisionSetAltitude(INS.Yaw, INS.Pitch); + VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180); } // temperature control - if ((count % 2) == 0) - { + if ((count % 2) == 0) { // 500hz IMU_Temperature_Ctrl(); } - if ((count++ % 1000) == 0) - { + if ((count++ % 1000) == 0) { // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 } } @@ -191,8 +182,7 @@ void INS_Task(void) * @param[2] vector in EarthFrame * @param[3] quaternion */ -void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) -{ +void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) { vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] + (q[1] * q[2] - q[0] * q[3]) * vecBF[1] + (q[1] * q[3] + q[0] * q[2]) * vecBF[2]); @@ -212,8 +202,7 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) * @param[2] vector in BodyFrame * @param[3] quaternion */ -void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) -{ +void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) { vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] + (q[1] * q[2] + q[0] * q[3]) * vecEF[1] + (q[1] * q[3] - q[0] * q[2]) * vecEF[2]); @@ -235,16 +224,14 @@ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) * @param gyro 角速度 * @param accel 加速度 */ -static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) -{ +static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) { static float lastYawOffset, lastPitchOffset, lastRollOffset; static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33; float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll; if (fabsf(param->Yaw - lastYawOffset) > 0.001f || fabsf(param->Pitch - lastPitchOffset) > 0.001f || - fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) - { + fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) { cosYaw = arm_cos_f32(param->Yaw / 57.295779513f); cosPitch = arm_cos_f32(param->Pitch / 57.295779513f); cosRoll = arm_cos_f32(param->Roll / 57.295779513f); @@ -304,8 +291,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[ /** * @brief Update quaternion */ -void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) -{ +void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) { float qa, qb, qc; gx *= 0.5f * dt; @@ -323,8 +309,7 @@ void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) /** * @brief Convert quaternion to eular angle */ -void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) -{ +void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) { *Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f; *Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f; *Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f; @@ -333,8 +318,7 @@ void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) /** * @brief Convert eular angle to quaternion */ -void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) -{ +void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) { float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll; Yaw /= 57.295779513f; Pitch /= 57.295779513f;