Simba-OmniInfantry/云台/modules/ins/ins.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]);
}