LSMushui
a4da5ad17c
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
56 lines
2.5 KiB
C
56 lines
2.5 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);
|
|
|
|
/* 坐标系变换工具(机体系 <-> 导航系) */
|
|
extern void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
|
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
|
|
|
#endif
|