Simba-OmniInfantry/云台/modules/sensor/bmi088.c

/* =============================================================================
 * bmi088.c - C 板板载六轴 IMU BMI088 驱动(HAL 版,架构与 legacy 一致)
 * 初始化(bmi088_init,任务上下文):
 *   1. 先关 EXTI4/EXTI9_5 NVIC(数据就绪中断若在初始化期间触发 DMA 读,
 *      会把 SPI 永久占在 BUSY,导致读 ID 全部失败--疯转/绿灯闪的根因)
 *   2. 重配 SPI1 为模式3(CPOL=1,CPHA=2边沿)/16 分频(5.25MHz,
 *      CubeMX 生成的是模式0/2分频 42MHz,BMI088 不支持)
 *   3. 有界重试读 ID(acc=0x1E, gyro=0x0F),成功后按 legacy 序列配置寄存器
 *   4. 清 EXTI 挂起标志后放行中断
 * 运行:  EXTI 下降沿 -> start_dma_read() SPI DMA 读取 -> TxRx 完成回调解析
 *        (加速度 8 字节 -> 链式读温度 4 字节;陀螺 7 字节),量程 ±3g/±2000dps
 * ========================================================================== */
#include "bmi088.h"
#include "main.h"
#include "spi.h"
#include "led.h"

/* ---- 寄存器(与 legacy 一致) ---- */
#define BMI088_ACC_CHIP_ID 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_RANGE 0x41
#define BMI088_INT1_IO_CONF 0x53
#define BMI088_INT1_INT2_MAP_DATA 0x58
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_DATA_START 0x12
#define BMI088_ACC_TEMP_MSB 0x22

#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_BANDWIDTH 0x10
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_INT_CTRL 0x15
#define BMI088_INT3_INT4_IO_CONF 0x16
#define BMI088_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_DATA_START 0x02

#define BMI088_ACC_ID_VALUE 0x1E
#define BMI088_GYRO_ID_VALUE 0x0F

#define BMI088_CS_ACC(x) HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define BMI088_CS_GYRO(x) HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)

float bmi088_accel[3];
float bmi088_gyro[3];
float bmi088_temperature;
uint8_t bmi088_online = 0;
uint8_t bmi088_last_acc_id = 0;
uint8_t bmi088_last_gyro_id = 0;
volatile uint32_t bmi088_accel_cnt = 0;
volatile uint32_t bmi088_gyro_cnt = 0;

static uint8_t spi_tx_buf[8];
static uint8_t spi_rx_buf[8];

/* 读数阶段(当前 SPI DMA 正在搬运的数据类型) */
typedef enum
{
    BMI088_PHASE_NONE = 0,
    BMI088_PHASE_ACCEL,
    BMI088_PHASE_TEMP,
    BMI088_PHASE_GYRO,
} bmi088_phase_e;

static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE;
/* 当前读的启动时刻,用于完成回调丢失时的超时兜底(见 EXTI 回调) */
static volatile uint32_t read_phase_tick = 0;

/* 释放当前读对应的片选 */
static void cs_release(bmi088_phase_e phase)
{
    if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
        BMI088_CS_ACC(1);
    else
        BMI088_CS_GYRO(1);
}

extern SPI_HandleTypeDef hspi1;

/* ---- 阻塞式寄存器读写 ---- */

static void accel_write_reg(uint8_t reg, uint8_t data)
{
    uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
    BMI088_CS_ACC(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
    BMI088_CS_ACC(1);
}

static uint8_t accel_read_reg(uint8_t reg)
{
    uint8_t tx[3] = {(uint8_t)(reg | 0x80), 0x00, 0x00};  // 加速度计首字节回读为乱码
    uint8_t rx[3];
    BMI088_CS_ACC(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 3, 10);
    BMI088_CS_ACC(1);
    return rx[2];
}

static void gyro_write_reg(uint8_t reg, uint8_t data)
{
    uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
    BMI088_CS_GYRO(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
    BMI088_CS_GYRO(1);
}

static uint8_t gyro_read_reg(uint8_t reg)
{
    uint8_t tx[2] = {(uint8_t)(reg | 0x80), 0x00};
    uint8_t rx[2];
    BMI088_CS_GYRO(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 10);
    BMI088_CS_GYRO(1);
    return rx[1];
}

/* ---- 数据解析(换算系数与 legacy 一致) ---- */

static void process_accel(void)
{
    int16_t raw[3];
    raw[0] = (int16_t)(((uint16_t)spi_rx_buf[3] << 8) | spi_rx_buf[2]);
    raw[1] = (int16_t)(((uint16_t)spi_rx_buf[5] << 8) | spi_rx_buf[4]);
    raw[2] = (int16_t)(((uint16_t)spi_rx_buf[7] << 8) | spi_rx_buf[6]);
    bmi088_accel[0] = raw[0] * 0.0008974358974f;  // (raw/32768*3)*9.8
    bmi088_accel[1] = raw[1] * 0.0008974358974f;
    bmi088_accel[2] = raw[2] * 0.0008974358974f;
    bmi088_accel_cnt++;
}

static void process_temperature(void)
{
    uint16_t raw = ((uint16_t)spi_rx_buf[2] << 3) | (spi_rx_buf[3] >> 5);  // 11bits
    int16_t temp = (raw > 1023) ? (int16_t)(raw - 2048) : (int16_t)raw;
    bmi088_temperature = temp * 0.125f + 23.0f;
}

static void process_gyro(void)
{
    int16_t raw[3];
    raw[0] = (int16_t)(((uint16_t)spi_rx_buf[2] << 8) | spi_rx_buf[1]);
    raw[1] = (int16_t)(((uint16_t)spi_rx_buf[4] << 8) | spi_rx_buf[3]);
    raw[2] = (int16_t)(((uint16_t)spi_rx_buf[6] << 8) | spi_rx_buf[5]);
    bmi088_gyro[0] = raw[0] * 0.0010652644360316952f;  // (raw/32768*2000)/180*π
    bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
    bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
    bmi088_gyro_cnt++;
}

/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */
static void start_dma_read(bmi088_phase_e phase, uint8_t reg, uint8_t len)
{
    if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
        return;

    read_phase = phase;
    read_phase_tick = HAL_GetTick();
    spi_tx_buf[0] = (uint8_t)(reg | 0x80);
    for (uint8_t i = 1; i < len; i++)
        spi_tx_buf[i] = 0x00;

    if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
        BMI088_CS_ACC(0);
    else
        BMI088_CS_GYRO(0);

    /* 启动失败必须回滚:此时 read_phase 已置位、片选已拉低,若直接返回,
     * 完成回调永远不会到来,read_phase 会永久停在非 NONE 状态,之后每个
     * 数据就绪中断都被 if (read_phase == NONE) 挡掉 —— 表现为 IMU 数据
     * 冻结在最后一个样本,而 bmi088_online 仍是 1(2026-09-12 排障) */
    if (HAL_SPI_TransmitReceive_DMA(&hspi1, spi_tx_buf, spi_rx_buf, len) != HAL_OK)
    {
        cs_release(phase);
        read_phase = BMI088_PHASE_NONE;
    }
}

/* ---- 初始化 ---- */

static void error_blink(void)
{
    /* BMI088 连接异常报警:绿灯连闪两下(经 TIM5 PWM,引脚在复用模式下
     * 直接写 GPIO 寄存器无效,必须走 LED 模块) */
    static uint8_t counter = 0;
    counter++;
    if (counter == 1)
        led_set_green(1);
    else if (counter == 5)
        led_set_green(0);
    else if (counter == 13)
        led_set_green(1);
    else if (counter == 17)
        led_set_green(0);
    else if (counter == 40)
        counter = 0;
    HAL_Delay(25);
}

void bmi088_init(void)
{
    /* 关键:数据就绪中断(EXTI)在芯片上电后就会持续触发,若不屏蔽,
     * EXTI 回调里的 SPI DMA 读会把 SPI 永远占在 BUSY,初始化的阻塞式
     * 读 ID 全部失败(legacy 靠初始化期间关闭同款 NVIC 规避,此处一致) */
    HAL_NVIC_DisableIRQ(EXTI4_IRQn);
    HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);

    /* CubeMX 生成的 SPI1 为模式0/2分频(42MHz),BMI088 需要模式3 且 <=10MHz,
     * 此处重新配置为模式3/16分频(5.25MHz,与 legacy 相同) */
    HAL_SPI_DeInit(&hspi1);
    hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
    hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
    if (HAL_SPI_Init(&hspi1) != HAL_OK)
        return;  // 放弃本次初始化,保持系统可运行

    BMI088_CS_ACC(1);
    BMI088_CS_GYRO(1);

    /* 检测 ID 并配置寄存器(与 legacy 序列一致)
     * 有界重试:最多 40 次(约 1s),失败则放弃并保持系统可运行 */
    for (uint8_t retry = 0; retry < 40; retry++)
    {
        bmi088_last_acc_id = accel_read_reg(BMI088_ACC_CHIP_ID);
        bmi088_last_gyro_id = gyro_read_reg(BMI088_GYRO_CHIP_ID);
        if (bmi088_last_acc_id == BMI088_ACC_ID_VALUE &&
            bmi088_last_gyro_id == BMI088_GYRO_ID_VALUE)
        {
            bmi088_online = 1;
            accel_write_reg(BMI088_ACC_SOFTRESET, 0xB6);
            HAL_Delay(1);  // 加速度计软重启 1ms
            gyro_write_reg(BMI088_GYRO_SOFTRESET, 0xB6);
            HAL_Delay(30);  // 陀螺仪软重启 30ms

            accel_read_reg(BMI088_ACC_CHIP_ID);  // 软重启后首笔读写有 bug,丢弃
            gyro_read_reg(BMI088_GYRO_CHIP_ID);

            accel_write_reg(BMI088_ACC_PWR_CTRL, 0x04);
            HAL_Delay(1);  // 上电等待(legacy 300us,放宽到 1ms)
            accel_write_reg(BMI088_ACC_PWR_CONF, 0x00);
            HAL_Delay(1);
            accel_write_reg(BMI088_ACC_CONF, 0xAB);   // 800Hz ODR,正常低通
            HAL_Delay(1);
            accel_write_reg(BMI088_ACC_RANGE, 0x00);  // ±3g
            HAL_Delay(1);
            accel_write_reg(BMI088_INT1_IO_CONF, 0x08);   // INT1 推挽,低有效
            HAL_Delay(1);
            accel_write_reg(BMI088_INT1_INT2_MAP_DATA, 0x04);  // INT1=数据就绪
            HAL_Delay(1);

            gyro_write_reg(BMI088_GYRO_RANGE, 0x00);      // ±2000°/s
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_BANDWIDTH, 0x82);  // 116Hz 带宽,1kHz ODR
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_LPM1, 0x00);       // 正常模式
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_INT_CTRL, 0x80);   // 使能数据就绪中断
            HAL_Delay(1);
            gyro_write_reg(BMI088_INT3_INT4_IO_CONF, 0x00);  // INT3 推挽,低有效
            HAL_Delay(1);
            gyro_write_reg(BMI088_INT3_INT4_IO_MAP, 0x01);   // INT3=数据就绪
            HAL_Delay(1);
            break;
        }
        error_blink();
    }

    /* 配置成功后清除挂起的数据就绪标志,再放行 EXTI 中断 */
    if (bmi088_online)
    {
        __HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
        __HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
        HAL_NVIC_EnableIRQ(EXTI4_IRQn);
        HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
    }
}

/* ---- 中断回调 ---- */

/* EXTI4(加速度计数据就绪)/ EXTI5(陀螺仪数据就绪) */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if (!bmi088_online)
        return;  // 初始化完成前不启动 DMA 读,避免占用 SPI

    /* 超时兜底:一次 SPI 读远小于 1ms,若超过 20ms 还没等到完成回调,
     * 说明回调丢了(SPI/DMA 异常),强行复位状态让数据路能自恢复,
     * 否则会永久冻结 —— 与上面 start_dma_read 的失败回滚同一考虑 */
    if (read_phase != BMI088_PHASE_NONE && (HAL_GetTick() - read_phase_tick) > 20U)
    {
        cs_release(read_phase);
        read_phase = BMI088_PHASE_NONE;
    }

    if (GPIO_Pin == INT1_ACCEL_Pin)
    {
        if (read_phase == BMI088_PHASE_NONE)
            start_dma_read(BMI088_PHASE_ACCEL, BMI088_ACC_DATA_START, 8);
    }
    else if (GPIO_Pin == INT1_GYRO_Pin)
    {
        if (read_phase == BMI088_PHASE_NONE)
            start_dma_read(BMI088_PHASE_GYRO, BMI088_GYRO_DATA_START, 7);
    }
}

/* SPI 收发完成 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
{
    if (hspi->Instance != SPI1)
        return;

    switch (read_phase)
    {
        case BMI088_PHASE_ACCEL:
            BMI088_CS_ACC(1);
            process_accel();
            read_phase = BMI088_PHASE_NONE;
            start_dma_read(BMI088_PHASE_TEMP, BMI088_ACC_TEMP_MSB, 4);  // 链式读取温度
            break;

        case BMI088_PHASE_TEMP:
            BMI088_CS_ACC(1);
            process_temperature();
            read_phase = BMI088_PHASE_NONE;
            break;

        case BMI088_PHASE_GYRO:
            BMI088_CS_GYRO(1);
            process_gyro();
            read_phase = BMI088_PHASE_NONE;
            break;

        default:
            break;
    }
}

/* ==========================================================================
 * 数据路健康检查与自恢复(任务上下文,由 attitude_update() 每 1ms 调用)
 * --------------------------------------------------------------------------
 * 背景(2026-09-12 现场):USB 遥测里 ca/cg 采样计数在开机约 1s 后彻底停止
 * 增长,而 imu=1、an/gn 看起来都正常 —— 数据冻结在最后一个样本上,姿态
 * 于是表现出"yaw 以恒定角速度自漂、pitch/roll 不动、陀螺不随板子移动"。
 *
 * 为什么必须有这个函数: 中断上下文里的兜底救不了这两类停摆——
 *   a) hspi1 被卡在 BUSY(某次 SPI/DMA 出错后 HAL 没回到 READY),
 *      start_dma_read() 的 HAL_SPI_GetState 检查会让每次都直接返回;
 *   b) BMI088 的 DRDY 是电平型信号,一旦某次失败/被挡掉的读取没把
 *      传感器的数据寄存器读走,INT 电平就保持住,之后既没有下降沿也没有
 *      上升沿 —— EXTI 再也不触发,写在 EXTI 回调里的任何兜底都不会执行。
 * 所以检查点放在任务里:停摆时先重启中断路(必要时 abort/重建 SPI),再用
 * 阻塞读保证本拍拿到真实数据。若中断路始终修不好,就退化成"每
 * BMI088_STALL_MS 补读一次"的轮询保底(约 200Hz),姿态不至于因为数据
 * 冻结而彻底失效;此时积分总量仍正确,只是运动解析粒度变粗。
 * 注意: dt 固定 1ms、1kHz 调用,所以即使采样只有 200Hz,一秒内积分出来
 * 的总转角依然是对的(保持值 × 0.001 × 1000 次 = 保持值)。
 * ========================================================================== */
#define BMI088_STALL_MS 5U  /* 陀螺 1kHz 输出,5ms 没新样本即可判停摆 */

volatile uint32_t bmi088_spi_errors = 0;

/* SPI 当前状态(HAL_SPI_StateTypeDef):1=READY,非 1 即被卡住,诊断用 */
uint8_t bmi088_spi_state(void)
{
    return (uint8_t)HAL_SPI_GetState(&hspi1);
}

/* 阻塞式读一次加速度计与陀螺仪数据(仅恢复路径使用:此时 EXTI 已关,不会
 * 与 DMA 抢 spi_rx_buf)。解析逻辑复用 process_accel/process_gyro */
static void bmi088_read_blocking(void)
{
    uint8_t tx[8];
    uint8_t rx[8];

    /* 加速度计:8 字节,首字节乱码,数据在 buf[2..7] */
    tx[0] = (uint8_t)(BMI088_ACC_DATA_START | 0x80);
    for (uint8_t i = 1; i < 8; i++)
        tx[i] = 0x00;
    BMI088_CS_ACC(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 8, 10);
    BMI088_CS_ACC(1);
    for (uint8_t i = 0; i < 8; i++)
        spi_rx_buf[i] = rx[i];
    process_accel();

    /* 陀螺仪:7 字节,数据在 buf[1..6] */
    tx[0] = (uint8_t)(BMI088_GYRO_DATA_START | 0x80);
    for (uint8_t i = 1; i < 7; i++)
        tx[i] = 0x00;
    BMI088_CS_GYRO(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 7, 10);
    BMI088_CS_GYRO(1);
    for (uint8_t i = 0; i < 7; i++)
        spi_rx_buf[i] = rx[i];
    process_gyro();
}

/* 重启中断路:复位相位/片选 -> 必要时重建 SPI -> 清 EXTI 挂起 -> 阻塞读一次 */
static void bmi088_recover(void)
{
    HAL_NVIC_DisableIRQ(EXTI4_IRQn);
    HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);

    cs_release(read_phase);
    read_phase = BMI088_PHASE_NONE;

    /* SPI 可能卡在 BUSY:先 abort,仍不 READY 就重建(注意 hspi1.Init 里
     * 仍是 bmi088_init 设好的模式3/16 分频,重建不会把时序改回去) */
    if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
        (void)HAL_SPI_Abort(&hspi1);
    if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
    {
        HAL_SPI_DeInit(&hspi1);
        (void)HAL_SPI_Init(&hspi1);
    }

    /* 清掉可能挂起的 EXTI 标志,否则旧电平会把下一次中断吞掉 */
    __HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
    __HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);

    bmi088_read_blocking();  /* 保证本拍有真实数据,同时清传感器 DRDY 电平 */

    HAL_NVIC_EnableIRQ(EXTI4_IRQn);
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}

/* 每 1ms 调用:采样计数没涨就说明中断路停了,恢复之 */
void bmi088_health_check(void)
{
    static uint32_t last_cnt = 0;
    static uint32_t stall_since = 0;
    const uint32_t now = HAL_GetTick();

    if (!bmi088_online)
        return;

    if (bmi088_gyro_cnt != last_cnt)
    {
        /* 正常:中断路在出新样本 */
        last_cnt = bmi088_gyro_cnt;
        stall_since = 0;
        return;
    }

    if (stall_since == 0)
    {
        stall_since = now;
        return;
    }
    if (now - stall_since < BMI088_STALL_MS)
        return;

    /* 停摆:重启中断路 + 阻塞读补一拍 */
    bmi088_recover();
    last_cnt = bmi088_gyro_cnt;
    stall_since = now;
}

/* SPI/DMA 出错回调:此前完全没有实现,出错后 SPI 状态可能停在 BUSY 而无人
 * 清理,数据路就此永久静止。这里至少把相位与片选复位,并计数以便遥测观察 */
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef * hspi)
{
    if (hspi->Instance != SPI1)
        return;

    cs_release(read_phase);
    read_phase = BMI088_PHASE_NONE;
    bmi088_spi_errors++;
}