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 的构建章节)。
63 lines
2.9 KiB
C
63 lines
2.9 KiB
C
/* =============================================================================
|
|
* ins.h - 惯导姿态解算封装(移植自 RoboMaster-C-Board-INS-Example, Wang Hongxi)
|
|
* 核心: QuaternionEKF 四元数 EKF——陀螺积分预测 + 加速度重力方向观测,
|
|
* 在线估计 x/y 陀螺零偏,卡方检验剔除加速度异常(撞击/运动)帧
|
|
* 对比旧 Mahony 方案: 收敛更快、零偏在线估计(无需上电静止校准)、
|
|
* 动态加速度干扰抑制更好(卡方检验)
|
|
* 数据源: 本工程 BMI088 驱动的 bmi088_accel/gyro(SPI DMA 后台更新)
|
|
* 调用: ins_init() 一次,ins_update(dt) 每 1ms;dt 用固定 0.001
|
|
* (IMU_Task 为 vTaskDelayUntil 精确 1kHz 节拍,无需 DWT 计时)
|
|
* ========================================================================== */
|
|
#ifndef INS_H
|
|
#define INS_H
|
|
|
|
#include <stdint.h>
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t Initialized;
|
|
|
|
float Gyro[3]; // rad/s(零偏修正后)
|
|
float Accel[3]; // m/s^2
|
|
float q[4]; // 四元数 w x y z
|
|
|
|
float Yaw; // -180~180(°)
|
|
float Pitch; // QEKF 命名(实际为绕 x 轴角,本工程映射见 attitude.c)
|
|
float Roll; // 同上
|
|
float YawTotalAngle; // 多圈连续 Yaw(°)
|
|
|
|
float GyroBias[3]; // EKF 在线估计的陀螺零偏(rad/s)
|
|
float MotionAccel_b[3]; // 运动加速度(机体系,去除重力,低通)
|
|
uint8_t StableFlag; // 1-载体静止且加速度正常(可用于 ZUPT 等扩展)
|
|
uint8_t ConvergeFlag; // 1-卡方检验收敛(姿态可信)
|
|
float ChiSquare; // 最近一次卡方检验值(诊断用)
|
|
uint32_t UpdateCount; // 更新计数(诊断用)
|
|
} INS_t;
|
|
|
|
extern INS_t INS;
|
|
|
|
/* 初始化:先做 z 轴静止零偏标定(约 1s 阻塞,车体必须静止——QEKF 的
|
|
* 卡方观测无法感知 z 偏置,GyroBias[2] 恒为 0,不标定会导致 Yaw 持续漂移),
|
|
* 再初始化 EKF(参数为 INS 例程 demo 值,调参见 QuaternionEKF.c 头注释) */
|
|
extern void ins_init(void);
|
|
|
|
/* 姿态更新(1kHz 调用,dt 单位秒;内部读 bmi088_accel/gyro 全局量)
|
|
* 标定完成前直接返回,不更新 */
|
|
extern void ins_update(float dt);
|
|
|
|
/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
|
|
extern uint8_t ins_ready(void);
|
|
|
|
/* ---- z 轴零偏标定的诊断量(见 ins.c 文件头) ----
|
|
* 姿态异常时先看这两个量和 debug_task 的 gz/cvg 是否自洽:
|
|
* 若 gyro_z_offset 明显偏离静止时的真实零偏,yaw 会以 -(offset) 的恒定
|
|
* 角速度自漂,与陀螺原始输出无关 */
|
|
extern float ins_gyro_z_offset; // 实际采用的 z 零偏(rad/s)
|
|
extern uint8_t ins_z_cal_ok; // 1-标定时车体静止,零偏可信;0-始终在动
|
|
|
|
/* 坐标系变换工具(机体系 <-> 导航系) */
|
|
extern void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
|
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
|
|
|
#endif
|