RMUL2025/User/device/bmi088.c

/*
        BMI088 陀螺仪+加速度计传感器。

*/

/* Includes ----------------------------------------------------------------- */
#include "bmi088.h"

#include <cmsis_os2.h>
#include <gpio.h>
#include <stdbool.h>
#include <string.h>

#include "bsp\delay.h"
#include "bsp\gpio.h"
#include "bsp\spi.h"
#include "component\user_math.h"

/* Private define ----------------------------------------------------------- */
#define BMI088_REG_ACCL_CHIP_ID (0x00)
#define BMI088_REG_ACCL_ERR (0x02)
#define BMI088_REG_ACCL_STATUS (0x03)
#define BMI088_REG_ACCL_X_LSB (0x12)
#define BMI088_REG_ACCL_X_MSB (0x13)
#define BMI088_REG_ACCL_Y_LSB (0x14)
#define BMI088_REG_ACCL_Y_MSB (0x15)
#define BMI088_REG_ACCL_Z_LSB (0x16)
#define BMI088_REG_ACCL_Z_MSB (0x17)
#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
#define BMI088_REG_ACCL_TEMP_MSB (0x22)
#define BMI088_REG_ACCL_TEMP_LSB (0x23)
#define BMI088_REG_ACCL_CONF (0x40)
#define BMI088_REG_ACCL_RANGE (0x41)
#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
#define BMI088_REG_ACCL_SELF_TEST (0x6D)
#define BMI088_REG_ACCL_PWR_CONF (0x7C)
#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
#define BMI088_REG_ACCL_SOFTRESET (0x7E)

#define BMI088_REG_GYRO_CHIP_ID (0x00)
#define BMI088_REG_GYRO_X_LSB (0x02)
#define BMI088_REG_GYRO_X_MSB (0x03)
#define BMI088_REG_GYRO_Y_LSB (0x04)
#define BMI088_REG_GYRO_Y_MSB (0x05)
#define BMI088_REG_GYRO_Z_LSB (0x06)
#define BMI088_REG_GYRO_Z_MSB (0x07)
#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
#define BMI088_REG_GYRO_RANGE (0x0F)
#define BMI088_REG_GYRO_BANDWIDTH (0x10)
#define BMI088_REG_GYRO_LPM1 (0x11)
#define BMI088_REG_GYRO_SOFTRESET (0x14)
#define BMI088_REG_GYRO_INT_CTRL (0x15)
#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
#define BMI088_REG_GYRO_SELF_TEST (0x3C)

#define BMI088_CHIP_ID_ACCL (0x1E)
#define BMI088_CHIP_ID_GYRO (0x0F)

#define BMI088_LEN_RX_BUFF (19)
/* Private macro ------------------------------------------------------------ */
#define BMI088_ACCL_NSS_SET() \
  HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET)
#define BMI088_ACCL_NSS_RESET() \
  HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_RESET)

#define BMI088_GYRO_NSS_SET() \
  HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET)
#define BMI088_GYRO_NSS_RESET() \
  HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_RESET)

/* Private typedef ---------------------------------------------------------- */
typedef enum {
  BMI_ACCL,
  BMI_GYRO,
} BMI_Device_t;

/* Private variables -------------------------------------------------------- */
static uint8_t buffer[2];
static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];

static osThreadId_t thread_alert;
static bool inited = false;

/* Private function  -------------------------------------------------------- */
static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
  buffer[0] = (reg & 0x7f);
  buffer[1] = data;

  BSP_Delay(1);
  switch (dv) {
    case BMI_ACCL:
      BMI088_ACCL_NSS_RESET();
      break;

    case BMI_GYRO:
      BMI088_GYRO_NSS_RESET();
      break;
  }

  HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 2u, 20u);

  switch (dv) {
    case BMI_ACCL:
      BMI088_ACCL_NSS_SET();
      break;

    case BMI_GYRO:
      BMI088_GYRO_NSS_SET();
      break;
  }
}

static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
  BSP_Delay(1);
  switch (dv) {
    case BMI_ACCL:
      BMI088_ACCL_NSS_RESET();
      break;

    case BMI_GYRO:
      BMI088_GYRO_NSS_RESET();
      break;
  }
  buffer[0] = (uint8_t)(reg | 0x80);
  HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 1u, 20u);
  HAL_SPI_Receive(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 2u, 20u);

  switch (dv) {
    case BMI_ACCL:
      BMI088_ACCL_NSS_SET();
      return buffer[1];

    case BMI_GYRO:
      BMI088_GYRO_NSS_SET();
      return buffer[0];
  }
}

static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
  if (data == NULL) return;

  switch (dv) {
    case BMI_ACCL:
      BMI088_ACCL_NSS_RESET();
      break;

    case BMI_GYRO:
      BMI088_GYRO_NSS_RESET();
      break;
  }
  buffer[0] = (uint8_t)(reg | 0x80);
  HAL_SPI_Transmit(BSP_SPI_GetHandle(BSP_SPI_IMU), buffer, 1u, 20u);
  HAL_SPI_Receive_DMA(BSP_SPI_GetHandle(BSP_SPI_IMU), data, len);
}

static void BMI088_RxCpltCallback(void) {
  if (HAL_GPIO_ReadPin(ACCL_CS_GPIO_Port, ACCL_CS_Pin) == GPIO_PIN_RESET) {
    BMI088_ACCL_NSS_SET();
    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
  }
  if (HAL_GPIO_ReadPin(GYRO_CS_GPIO_Port, GYRO_CS_Pin) == GPIO_PIN_RESET) {
    BMI088_GYRO_NSS_SET();
    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
  }
}

static void BMI088_AcclIntCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
}

static void BMI088_GyroIntCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
}

/* Exported functions ------------------------------------------------------- */
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
  if (bmi088 == NULL) return DEVICE_ERR_NULL;
  if (cali == NULL) return DEVICE_ERR_NULL;
  if (inited) return DEVICE_ERR_INITED;
  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  bmi088->cali = cali;

  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
  BSP_Delay(30);

  /* Switch accl to SPI mode. */
  BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);

  if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
    return DEVICE_ERR_NO_DEV;

  if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
    return DEVICE_ERR_NO_DEV;

  BSP_GPIO_DisableIRQ(ACCL_INT_Pin);
  BSP_GPIO_DisableIRQ(GYRO_INT_Pin);

  BSP_SPI_RegisterCallback(BSP_SPI_IMU, BSP_SPI_RX_CPLT_CB,
                           BMI088_RxCpltCallback);
  BSP_GPIO_RegisterCallback(ACCL_INT_Pin, BMI088_AcclIntCallback);
  BSP_GPIO_RegisterCallback(GYRO_INT_Pin, BMI088_GyroIntCallback);

  /* Accl init. */
  /* Filter setting: Normal. */
  /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);

  /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);

  /* INT1 as output. Push-pull. Active low. Output. */
  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);

  /* Map data ready interrupt to INT1. */
  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);

  /* Turn on accl. Now we can read data. */
  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
  BSP_Delay(50);

  /* Gyro init. */
  /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);

  /* Filter bw: 47Hz. */
  /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);

  /* INT3 and INT4 as output. Push-pull. Active low. */
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);

  /* Map data ready interrupt to INT3. */
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);

  /* Enable new data interrupt. */
  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);

  BSP_Delay(10);

  inited = true;

  BSP_GPIO_EnableIRQ(ACCL_INT_Pin);
  BSP_GPIO_EnableIRQ(GYRO_INT_Pin);
  return DEVICE_OK;
}

bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
  return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
}

uint32_t BMI088_WaitNew() {
  return osThreadFlagsWait(
      SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
      osWaitForever);
}

int8_t BMI088_AcclStartDmaRecv() {
  BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
  return DEVICE_OK;
}

uint32_t BMI088_AcclWaitDmaCplt() {
  return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
                           osWaitForever);
}

int8_t BMI088_GyroStartDmaRecv() {
  BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
  return DEVICE_OK;
}

uint32_t BMI088_GyroWaitDmaCplt() {
  return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
                           osWaitForever);
}

int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
  if (bmi088 == NULL) return DEVICE_ERR_NULL;

#if 1
  int16_t raw_x, raw_y, raw_z;
  memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
  memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
  memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));

  bmi088->accl.x = (float)raw_x;
  bmi088->accl.y = (float)raw_y;
  bmi088->accl.z = (float)raw_z;

#else
  const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
  const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
  const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);

  bmi088->accl.x = (float)*praw_x;
  bmi088->accl.y = (float)*praw_y;
  bmi088->accl.z = (float)*praw_z;

#endif

  /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
  bmi088->accl.x /= 5460.0f;
  bmi088->accl.y /= 5460.0f;
  bmi088->accl.z /= 5460.0f;

  int16_t raw_temp =
      (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));

  if (raw_temp > 1023) raw_temp -= 2048;

  bmi088->temp = (float)raw_temp * 0.125f + 23.0f;

  return DEVICE_OK;
}

int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
  if (bmi088 == NULL) return DEVICE_ERR_NULL;

#if 1
  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
  int16_t raw_x, raw_y, raw_z;
  memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
  memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
  memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));

  bmi088->gyro.x = (float)raw_x;
  bmi088->gyro.y = (float)raw_y;
  bmi088->gyro.z = (float)raw_z;

#else
  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
  const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
  const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
  const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);

  bmi088->gyro.x = (float)*raw_x;
  bmi088->gyro.y = (float)*raw_y;
  bmi088->gyro.z = (float)*raw_z;
#endif

  /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
   * FS2000: 16.384.*/
  bmi088->gyro.x /= 32.768f;
  bmi088->gyro.y /= 32.768f;
  bmi088->gyro.z /= 32.768f;

  bmi088->gyro.x *= M_DEG2RAD_MULT;
  bmi088->gyro.y *= M_DEG2RAD_MULT;
  bmi088->gyro.z *= M_DEG2RAD_MULT;

  bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
  bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
  bmi088->gyro.z -= bmi088->cali->gyro_offset.z;

  return DEVICE_ERR_NULL;
}

float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
  (void)bmi088;
  return 400.0f;
}