/* ============================================================================= * ins.c - 惯导姿态解算封装实现(见 ins.h 说明) * 移植自 RoboMaster-C-Board-INS-Example-master 的 ins_task.c: * - 去除 BMI088driver/controller/bsp_PWM/bsp_dwt 依赖,数据源改为本工程 * 的 bmi088 驱动全局量,温度控制仍由 IMU_Task 负责 * - dt 由调用方传入(IMU_Task 固定 1ms),不再使用 DWT 计时 * - 保留运动加速度解算(低通 0.0085),供后续功能扩展(如功率/跌倒检测) * 欧拉角约定(重要): * QEKF 内部变量命名与航空惯例互换——QEKF_INS.Pitch 是绕 x 轴的角, * QEKF_INS.Roll 是绕 y 轴的角;attitude.c 按"本工程 Pitch 反馈=QEKF.Pitch" * 映射(与旧 Mahony 方案的 get_roll 语义一致),勿凭名字想当然 * ========================================================================== */ #include "ins.h" #include "QuaternionEKF.h" #include "bmi088.h" #include "main.h" #include /* ---- z 轴静止零偏标定 ---- * QEKF 无法观测 z 偏置(见 ins.h),只能上电静止标定。 * 2026-09-12 加健壮性:原来直接对前 1000 个样本取均值,而 BMI088 刚初始化 * 完时 SPI DMA 还没稳定,且上电瞬间车体往往正被摆弄 —— 一旦标定窗口里混入 * 运动样本,gyro_z_offset 就被污染成一个偏大的常值,表现为**上电后 yaw 以 * 恒定角速度自漂**(实测 -3.85°/s,而原始 |gyro| 仅 0.017rad/s,量级对不上 * 才发现是零偏而非陀螺本身)。现在: * 1. 先等采样路稳定(INS_Z_CAL_SETTLE_MS) * 2. 单样本 |gyro| 超过 INS_Z_CAL_MAX_G 视为无效(静止时不该出现) * 3. 一轮内有效样本占比不足 INS_Z_CAL_MIN_QUIET 视为"上电时车体在动", * 整轮作废重测,最多 INS_Z_CAL_ATTEMPTS 轮 * 4. 多轮都不静则用最后一轮的均值并置 ins_z_cal_ok=0(有补偿总好过没有, * 但是否可信由该标志告诉调用方/调试者) * 单轮只取 0.5s:LED_Task 的 IWDG 上电宽限期是 5s,而 IMU_Task 在 * attitude_init() 期间不刷新心跳,所以整个初始化必须留在 5s 内 —— 可靠 * 性靠"整轮作废重测"而不是靠拉长单轮,3 轮最坏 1.55s,仅比原来的 1s 略长 */ #define INS_Z_CAL_SETTLE_MS 50 #define INS_Z_CAL_SAMPLES 500 #define INS_Z_CAL_MAX_G 0.35f /* rad/s,约 20°/s */ #define INS_Z_CAL_MIN_QUIET 0.9f #define INS_Z_CAL_ATTEMPTS 3 static float gyro_z_offset = 0; static uint8_t z_cal_done = 0; float ins_gyro_z_offset = 0; /* 诊断用,见 ins.h */ uint8_t ins_z_cal_ok = 0; /* 诊断用,见 ins.h */ INS_t INS; static const float xb[3] = {1, 0, 0}; static const float yb[3] = {0, 1, 0}; static const float zb[3] = {0, 0, 1}; void ins_init(void) { memset(&INS, 0, sizeof(INS)); /* 等 SPI 采样路稳定,避免把未就绪/残留的样本算进零偏 */ HAL_Delay(INS_Z_CAL_SETTLE_MS); /* z 轴静止零偏标定(见文件头;x/y 偏置由 EKF 在线估计,无需校准)。 * 标定期间 ins_update() 不输出,控制任务由 ins_ready() 门控 */ for (uint8_t attempt = 0; attempt < INS_Z_CAL_ATTEMPTS; attempt++) { float sum = 0; uint16_t valid = 0; for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++) { const float gx = bmi088_gyro[0]; const float gy = bmi088_gyro[1]; const float gz = bmi088_gyro[2]; if (gx * gx + gy * gy + gz * gz <= INS_Z_CAL_MAX_G * INS_Z_CAL_MAX_G) { sum += gz; valid++; } HAL_Delay(1); } if (valid > 0) gyro_z_offset = sum / (float)valid; if ((float)valid >= INS_Z_CAL_MIN_QUIET * (float)INS_Z_CAL_SAMPLES) { ins_z_cal_ok = 1; /* 整轮都静止,零偏可信 */ break; } /* 否则认为上电时车体在动,整轮作废、再来一轮 */ } ins_gyro_z_offset = gyro_z_offset; z_cal_done = 1; INS.Initialized = 1; /* 与 INS 例程 demo 相同的参数组合: * 过程噪声1=10(四元数) 过程噪声2=0.001(零偏) 观测噪声=1e7(加速度) * 渐消因子=1(不渐消) 加速度低通=0(内部另有运动加速度低通) */ IMU_QuaternionEKF_Init(10, 0.001f, 10000000, 1, 0); } uint8_t ins_ready(void) { return z_cal_done; } void ins_update(float dt) { if (!z_cal_done) return; /* BMI088 驱动在 SPI DMA 完成回调中后台更新,这里直接取最新值; * z 轴减去上电标定的零偏(x/y 由 EKF 在线估计,不再静态校准) */ float gyro[3] = {bmi088_gyro[0], bmi088_gyro[1], bmi088_gyro[2] - gyro_z_offset}; float accel[3] = {bmi088_accel[0], bmi088_accel[1], bmi088_accel[2]}; /* 核心函数,EKF 更新四元数(内部含零偏估计与卡方检验) */ IMU_QuaternionEKF_Update(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], dt); memcpy(INS.q, QEKF_INS.q, sizeof(INS.q)); /* 机体系基向量转换到导航系(惯性系=导航系),供扩展功能使用 */ float xn[3], yn[3], zn[3]; BodyFrameToEarthFrame(xb, xn, INS.q); BodyFrameToEarthFrame(yb, yn, INS.q); BodyFrameToEarthFrame(zb, zn, INS.q); /* 将重力从导航系转到机体系,计算运动加速度(低通),供扩展使用 */ const float gravity[3] = {0, 0, 9.81f}; float gravity_b[3]; EarthFrameToBodyFrame(gravity, gravity_b, INS.q); const float AccelLPF = 0.0085f; for (uint8_t i = 0; i < 3; i++) { INS.MotionAccel_b[i] = (accel[i] - gravity_b[i]) * dt / (AccelLPF + dt) + INS.MotionAccel_b[i] * AccelLPF / (AccelLPF + dt); } /* 获取最终数据(QEKF 内部已含多圈 Yaw 处理) */ INS.Yaw = QEKF_INS.Yaw; INS.Pitch = QEKF_INS.Pitch; INS.Roll = QEKF_INS.Roll; INS.YawTotalAngle = QEKF_INS.YawTotalAngle; INS.GyroBias[0] = QEKF_INS.GyroBias[0]; INS.GyroBias[1] = QEKF_INS.GyroBias[1]; INS.GyroBias[2] = QEKF_INS.GyroBias[2]; INS.StableFlag = QEKF_INS.StableFlag; INS.ConvergeFlag = QEKF_INS.ConvergeFlag; INS.ChiSquare = QEKF_INS.ChiSquare_Data[0]; INS.UpdateCount = QEKF_INS.UpdateCount; (void)xn; (void)yn; (void)zn; (void)zb; } /** * @brief Transform 3dvector from BodyFrame to EarthFrame * @param[1] vector in BodyFrame * @param[2] vector in EarthFrame * @param[3] quaternion */ 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]); vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] + (q[2] * q[3] - q[0] * q[1]) * vecBF[2]); vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] + (q[2] * q[3] + q[0] * q[1]) * vecBF[1] + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]); } /** * @brief Transform 3dvector from EarthFrame to BodyFrame * @param[1] vector in EarthFrame * @param[2] vector in BodyFrame * @param[3] quaternion */ 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]); vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] + (q[2] * q[3] + q[0] * q[1]) * vecEF[2]); vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] + (q[2] * q[3] - q[0] * q[1]) * vecEF[1] + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]); }