LSMushui
e3d432f3b5
原 modules/ 下目录大小写混用(BMI088/BUZZER/DR16/IST8310/LED/PID 为大写,
attitude/c_comm/filter/gimbal/ins/motor/referee/shoot 为小写),且把第三方
移植代码(QuaternionEKF/kalman_filter)与本工程封装(ins)混在同一目录。
新结构(全部小写,按角色分层,第三方代码单独隔离):
modules/
algorithm/ pid.c/h filter.c/h 通用算法
imu/ attitude.c/h ins.c/h 姿态与惯导封装
imu/qekf/ QuaternionEKF.c/h kalman_filter.c/h
(移植自 Wang Hongxi INS 例程)
sensor/ bmi088.c/h ist8310.c/h 板载传感器驱动
comms/ dr16.c/h c_comm.c/h referee.c/h
motor/ dji_motor.c/h 电机抽象
control/ gimbal.c/h shoot.c/h 整车控制逻辑
(底盘为 chassis.c/h)
dev/ led.c/h buzzer.c/h 板级指示/发声
共移动 58 个文件(每板 29 个),旧目录全部删除。
工程文件同步(每个工程各 2 个):
- .uvprojx: Modules 分组 16 个 <FilePath> 改为新路径;
2 个非空 <IncludePath> 元素:15 个旧模块路径收敛为 8 个新目录,
并顺带删除陈旧项 ../modules/AHRS(目录根本不存在,legacy 时代残留)
与冗余项 ..\bsp(与 ../Bsp/Inc 重复)
- .uvoptx: 16 个 <PathWithFileName> 同步
关于"为什么不用改源码": Keil 的 include 靠 IncludePath 解析,本工程所有
#include 都是纯文件名形式(无路径限定),所以移动目录只需改工程配置。
另外 .h 完全不登记在工程里(只有 16 个 .c 被登记),故头文件只需移动。
回归验证(无法在此环境编译,用等价判据):
- 两个工程引用的 98 / 95 个 <FilePath> 全部真实存在
- 磁盘上 47 / 45 个 .c 全部仍被工程引用 —— 即没有任何文件掉出编译
- 2 个 IncludePath 元素均含全部 8 个新模块目录,无旧路径残留
- 所有 IncludePath 指向的目录均真实存在
- .uvprojx/.uvoptx XML 良构
- 两棵树结构一致(chassis/gimbal 等板级差异符合预期)
⚠️ 仍需在 Keil 里点一次编译确认(见 README 的构建章节)。
199 lines
7.9 KiB
C
199 lines
7.9 KiB
C
/* =============================================================================
|
|
* 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 <string.h>
|
|
|
|
/* ---- 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]);
|
|
}
|