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,避免固件源码在检出时被改动换行符
153 lines
5.7 KiB
C
153 lines
5.7 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),上电采 1s */
|
|
#define INS_Z_CAL_SAMPLES 1000
|
|
static float gyro_z_offset = 0;
|
|
static uint8_t z_cal_done = 0;
|
|
|
|
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));
|
|
|
|
/* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计,
|
|
* 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */
|
|
float sum = 0;
|
|
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
|
|
{
|
|
sum += bmi088_gyro[2];
|
|
HAL_Delay(1);
|
|
}
|
|
gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
|
|
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]);
|
|
}
|