From f8fbde6b6ab0f87482d3b25f45161e538145bb4b Mon Sep 17 00:00:00 2001 From: EMSR Date: Sat, 4 Nov 2023 18:09:36 +0800 Subject: [PATCH] add bmi323 imu support --- src/driver/spi_bmi323.c | 187 +++++++++++++++++++++++++++++++++++ src/driver/spi_bmi323.h | 214 ++++++++++++++++++++++++++++++++++++++++ src/driver/spi_gyro.c | 16 ++- src/driver/spi_gyro.h | 1 + 4 files changed, 417 insertions(+), 1 deletion(-) create mode 100644 src/driver/spi_bmi323.c create mode 100644 src/driver/spi_bmi323.h diff --git a/src/driver/spi_bmi323.c b/src/driver/spi_bmi323.c new file mode 100644 index 000000000..6ac3ceb36 --- /dev/null +++ b/src/driver/spi_bmi323.c @@ -0,0 +1,187 @@ +#include "driver/spi_bmi323.h" + +#include "core/profile.h" +#include "driver/gpio.h" +#include "driver/spi.h" +#include "driver/spi_gyro.h" +#include "driver/time.h" +#include "util/util.h" + +#ifdef USE_GYRO + +#define SPI_SPEED_SLOW MHZ_TO_HZ(4) +#define SPI_SPEED_FAST MHZ_TO_HZ(10) + +extern spi_bus_device_t gyro_bus; + +static int8_t gyro_cas = 0; // not ready + +static void bmi323_reset_to_spi() { + // put the device in spi mode by toggeling CS + gpio_pin_reset(gyro_bus.nss); + time_delay_ms(1); + gpio_pin_set(gyro_bus.nss); + time_delay_ms(10); +} + +uint8_t bmi323_detect() { + bmi323_reset_to_spi(); + + const uint8_t whoami = bmi3_read8(BMI323_REG_CHIP_ID); + switch (whoami) { + case BMI323_WHO_AMI: + return GYRO_TYPE_BMI323; + + default: + return GYRO_TYPE_INVALID; + } +} + +static void bmi323_init() { + bmi323_reset_to_spi(); + bmi3_write16(BMI323_REG_CMD, BMI323_CMD_SOFT_RESET, 100); + bmi323_reset_to_spi(); +} + +static void bmi323_init_config() { + // init acc conf + uint16_t regdata = 0; + regdata = BMI3_ACC_BW_ODR_QUARTER << 7 | BMI323_ACC_RANGE_16G << 4 | BMI323_ACC_ODR_6400HZ; + regdata |= BMI3_ACC_MODE_HIGH_PERF << 12 | BMI323_ACC_AVG1 << 8; + bmi3_write16(BMI323_REG_ACC_CONF, regdata, 1); + + // init gyro conf + regdata = 0; + regdata = BMI323_GYR_BW_ODR_QUARTER << 7 | BMI323_GYR_RANGE_2000DPS << 4 | BMI323_GYR_ODR_6400HZ; + regdata |= BMI323_GYR_MODE_HIGH_PERF << 12 | BMI323_GYR_AVG1 << 8; + bmi3_write16(BMI323_REG_GYRO_CONF, regdata, 1); + + // init data ready interupt to pin int1/ push_pull /active_high NO_LATCH(default) + // BMI323_REG_IO_INT_CTRL push_pull/active_high + bmi3_write8(BMI323_REG_IO_INT_CTRL, BMI3_INT_OUTPUT_ENABLE << 2 | BMI3_INT_PUSH_PULL << 1 | BMI3_INT_ACTIVE_HIGH, 1); + + // BMI323_REG_INT_MAP2 accready /gyro_ready + regdata = 0; + regdata = 0x01 << 10 | 0x01 << 8; + bmi3_write16(BMI323_REG_INT_MAP2, regdata, 15); +} + +void bmi323_configure() { + bmi323_init(); + // skip CRT for now + bmi323_init_config(); + // bmi323_enable_cas(); +} + +uint8_t bmi3_read8(uint8_t reg) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_SLOW); + + uint8_t buffer[3] = {reg | 0x80, 0x0, 0x0}; + + const spi_txn_segment_t segs[] = { + spi_make_seg_buffer(buffer, buffer, 3), + }; + spi_seg_submit_wait(&gyro_bus, segs); + + return buffer[2]; +} + +uint16_t bmi3_read16(uint8_t reg) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_SLOW); + + uint8_t buffer[4] = {reg | 0x80, 0x0, 0x0, 0x0}; + + const spi_txn_segment_t segs[] = { + spi_make_seg_buffer(buffer, buffer, 4), + }; + spi_seg_submit_wait(&gyro_bus, segs); + + return ((buffer[3] << 8) | buffer[2]); +} + +void bmi3_write8(uint8_t reg, uint8_t data, uint32_t delay) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_SLOW); + + const spi_txn_segment_t segs[] = { + spi_make_seg_const(reg), + spi_make_seg_const(data), + }; + spi_seg_submit_wait(&gyro_bus, segs); + + time_delay_ms(delay); +} + +void bmi3_write16(uint8_t reg, uint16_t data, uint32_t delay) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_SLOW); + + const spi_txn_segment_t segs[] = { + spi_make_seg_const(reg), + spi_make_seg_const(data & 0xff), + spi_make_seg_const(data >> 8), + }; + spi_seg_submit_wait(&gyro_bus, segs); + + time_delay_ms(delay); +} + +void bmi3_write_data(uint8_t reg, uint8_t *data, uint32_t size, uint32_t delay) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_SLOW); + + const spi_txn_segment_t segs[] = { + spi_make_seg_const(reg), + spi_make_seg_buffer(NULL, data, size), + }; + spi_seg_submit_wait(&gyro_bus, segs); + + time_delay_ms(delay); +} + +void bmi3_read_data(uint8_t reg, uint8_t *data, uint32_t size) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST); + + const spi_txn_segment_t segs[] = { + spi_make_seg_const(reg | 0x80), + spi_make_seg_const(0xFF), + spi_make_seg_buffer(data, NULL, size), + }; + spi_seg_submit_wait(&gyro_bus, segs); +} + +void bmi323_read_gyro_data(gyro_data_t *data) { + spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST); + + uint8_t buf[12]; + const spi_txn_segment_t gyro_segs[] = { + spi_make_seg_const(BMI323_REG_ACC_DATA_X_LSB | 0x80), + spi_make_seg_const(0xFF), + spi_make_seg_buffer(buf, NULL, 12), + }; + spi_seg_submit_wait(&gyro_bus, gyro_segs); + + data->accel.axis[0] = -(int16_t)((buf[1] << 8) | buf[0]); + data->accel.axis[1] = -(int16_t)((buf[3] << 8) | buf[2]); + data->accel.axis[2] = (int16_t)((buf[5] << 8) | buf[4]); + + int16_t gyro_data[3] = { + (int16_t)((buf[7] << 8) | buf[6]), + (int16_t)((buf[9] << 8) | buf[8]), + (int16_t)((buf[11] << 8) | buf[10]), + }; + + const int32_t tempx = gyro_data[0] - (int16_t)(gyro_cas * (int16_t)(gyro_data[2]) / 512); + if (tempx > 32767) { + gyro_data[0] = 32767; + } else if (tempx < -32768) { + gyro_data[0] = -32768; + } else { + gyro_data[0] = tempx; + } + + data->gyro.axis[1] = gyro_data[0]; + data->gyro.axis[0] = gyro_data[1]; + data->gyro.axis[2] = gyro_data[2]; + + data->temp = 0; +} + +#endif \ No newline at end of file diff --git a/src/driver/spi_bmi323.h b/src/driver/spi_bmi323.h new file mode 100644 index 000000000..e33602a75 --- /dev/null +++ b/src/driver/spi_bmi323.h @@ -0,0 +1,214 @@ +#pragma once + +#include + +#include "driver/spi_gyro.h" + +#define BMI323_REG_CHIP_ID 0x00 +#define BMI323_WHO_AMI 0x43 +#define BMI323_REG_ERR_REG 0x01 +#define BMI323_REG_STATUS 0x03 + +#define BMI323_REG_ACC_DATA_X_LSB 0x03 + +#define BMI323_REG_SENSORTIME_0 0x0A +#define BMI323_REG_SENSORTIME_1 0x0B +// Overflow state +#define BMI323_REG_SAT_FLAGS 0x0C + +// ACC_CONF GYRO_CONF 16bit +#define BMI323_REG_ACC_CONF 0x20 +#define BMI323_REG_GYRO_CONF 0x21 + +// Interupt status for pull +#define BMI323_REG_INT_STATUS_INT1 0x0D +#define BMI323_REG_INT_STATUS_INT2 0x0E +#define BMI323_REG_INT_STATUS_IBI 0x0F + +// Interupt output conf +#define BMI323_REG_IO_INT_CTRL 0x38 +#define BMI323_REG_INT_LATCH_CONF 0x39 +#define BMI323_REG_INT_MAP2 0x3B + +// SPI3 CONF +#define BMI323_REG_IO_SPI_IF 0x50 +#define BMI323_DAT_IO_SPI_3_EN 0x01 +#define BMI323_DAT_IO_SPI_4_DEFAULT 0x00 + +// OFFSET AND GAIN FOR CALIBRATION +#define BMI323_REG_ACC_DP_OFF_X 0x60 +#define BMI323_REG_ACC_DP_DGAIN_X 0x61 +#define BMI323_REG_ACC_DP_OFF_Y 0x62 +#define BMI323_REG_ACC_DP_DGAIN_Y 0x63 +#define BMI323_REG_ACC_DP_OFF_Z 0x64 +#define BMI323_REG_ACC_DP_DGAIN_Z 0x65 + +#define BMI323_REG_GYRO_DP_OFF_X 0x66 +#define BMI323_REG_GYRO_DP_DGAIN_X 0x67 +#define BMI323_REG_GYRO_DP_OFF_Y 0x68 +#define BMI323_REG_GYRO_DP_DGAIN_Y 0x69 +#define BMI323_REG_GYRO_DP_OFF_Z 0x6A +#define BMI323_REG_GYRO_DP_DGAIN_Z 0x6B + +#define BMI323_REG_CMD 0x7E +#define BMI323_REG_CFG_RES 0x7F + +/******************************************************************************/ +/*! Accelerometer Macro Definitions For CONF */ +/******************************************************************************/ +/*! Accelerometer Bandwidth parameters */ +#define BMI323_ACC_AVG1 UINT8_C(0x00) +#define BMI323_ACC_AVG2 UINT8_C(0x01) +#define BMI323_ACC_AVG4 UINT8_C(0x02) +#define BMI323_ACC_AVG8 UINT8_C(0x03) +#define BMI323_ACC_AVG16 UINT8_C(0x04) +#define BMI323_ACC_AVG32 UINT8_C(0x05) +#define BMI323_ACC_AVG64 UINT8_C(0x06) + +/*! Accelerometer Output Data Rate */ +#define BMI323_ACC_ODR_0_78HZ UINT8_C(0x01) +#define BMI323_ACC_ODR_1_56HZ UINT8_C(0x02) +#define BMI323_ACC_ODR_3_125HZ UINT8_C(0x03) +#define BMI323_ACC_ODR_6_25HZ UINT8_C(0x04) +#define BMI323_ACC_ODR_12_5HZ UINT8_C(0x05) +#define BMI323_ACC_ODR_25HZ UINT8_C(0x06) +#define BMI323_ACC_ODR_50HZ UINT8_C(0x07) +#define BMI323_ACC_ODR_100HZ UINT8_C(0x08) +#define BMI323_ACC_ODR_200HZ UINT8_C(0x09) +#define BMI323_ACC_ODR_400HZ UINT8_C(0x0A) +#define BMI323_ACC_ODR_800HZ UINT8_C(0x0B) +#define BMI323_ACC_ODR_1600HZ UINT8_C(0x0C) +#define BMI323_ACC_ODR_3200HZ UINT8_C(0x0D) +#define BMI323_ACC_ODR_6400HZ UINT8_C(0x0E) + +/*! Accelerometer G Range */ +#define BMI323_ACC_RANGE_2G UINT8_C(0x00) +#define BMI323_ACC_RANGE_4G UINT8_C(0x01) +#define BMI323_ACC_RANGE_8G UINT8_C(0x02) +#define BMI323_ACC_RANGE_16G UINT8_C(0x03) + +/*! Accelerometer mode */ +#define BMI3_ACC_MODE_DISABLE UINT8_C(0x00) +#define BMI3_ACC_MODE_LOW_PWR UINT8_C(0x03) +#define BMI3_ACC_MODE_NORMAL UINT8_C(0X04) +#define BMI3_ACC_MODE_HIGH_PERF UINT8_C(0x07) + +/*! Accelerometer bandwidth */ +#define BMI3_ACC_BW_ODR_HALF UINT8_C(0) +#define BMI3_ACC_BW_ODR_QUARTER UINT8_C(1) + +/******************************************************************************/ +/*! Gyroscope Macro Definitions For Conf */ +/******************************************************************************/ +/*! Gyroscope Bandwidth parameters */ +#define BMI323_GYR_AVG1 UINT8_C(0x00) +#define BMI323_GYR_AVG2 UINT8_C(0x01) +#define BMI323_GYR_AVG4 UINT8_C(0x02) +#define BMI323_GYR_AVG8 UINT8_C(0x03) +#define BMI323_GYR_AVG16 UINT8_C(0x04) +#define BMI323_GYR_AVG32 UINT8_C(0x05) +#define BMI323_GYR_AVG64 UINT8_C(0x06) + +/*! Gyroscope Output Data Rate */ +#define BMI323_GYR_ODR_0_78HZ UINT8_C(0x01) +#define BMI323_GYR_ODR_1_56HZ UINT8_C(0x02) +#define BMI323_GYR_ODR_3_125HZ UINT8_C(0x03) +#define BMI323_GYR_ODR_6_25HZ UINT8_C(0x04) +#define BMI323_GYR_ODR_12_5HZ UINT8_C(0x05) +#define BMI323_GYR_ODR_25HZ UINT8_C(0x06) +#define BMI323_GYR_ODR_50HZ UINT8_C(0x07) +#define BMI323_GYR_ODR_100HZ UINT8_C(0x08) +#define BMI323_GYR_ODR_200HZ UINT8_C(0x09) +#define BMI323_GYR_ODR_400HZ UINT8_C(0x0A) +#define BMI323_GYR_ODR_800HZ UINT8_C(0x0B) +#define BMI323_GYR_ODR_1600HZ UINT8_C(0x0C) +#define BMI323_GYR_ODR_3200HZ UINT8_C(0x0D) +#define BMI323_GYR_ODR_6400HZ UINT8_C(0x0E) + +/*! Gyroscope DPS Range */ +#define BMI323_GYR_RANGE_125DPS UINT8_C(0x00) +#define BMI323_GYR_RANGE_250DPS UINT8_C(0x01) +#define BMI323_GYR_RANGE_500DPS UINT8_C(0x02) +#define BMI323_GYR_RANGE_1000DPS UINT8_C(0x03) +#define BMI323_GYR_RANGE_2000DPS UINT8_C(0x04) + +/*! Gyroscope mode */ +#define BMI323_GYR_MODE_DISABLE UINT8_C(0x00) +#define BMI323_GYR_MODE_SUSPEND UINT8_C(0X01) +#define BMI323_GYR_MODE_LOW_PWR UINT8_C(0x03) +#define BMI323_GYR_MODE_NORMAL UINT8_C(0X04) +#define BMI323_GYR_MODE_HIGH_PERF UINT8_C(0x07) + +/*! Gyroscope bandwidth */ +#define BMI323_GYR_BW_ODR_HALF UINT8_C(0) +#define BMI323_GYR_BW_ODR_QUARTER UINT8_C(1) + +/******************************************************************************/ +/*! @name BMI3 Interrupt Modes Conf*/ +/******************************************************************************/ +/*! Non latched */ +#define BMI3_INT_NON_LATCH UINT8_C(0) + +#define BMI3_INT_LATCH_EN UINT8_C(1) +#define BMI3_INT_LATCH_DISABLE UINT8_C(0) + +/*! BMI3 Interrupt Pin Behavior */ +#define BMI3_INT_PUSH_PULL UINT8_C(0) +#define BMI3_INT_OPEN_DRAIN UINT8_C(1) + +/*! BMI3 Interrupt Pin Level */ +#define BMI3_INT_ACTIVE_LOW UINT8_C(0) +#define BMI3_INT_ACTIVE_HIGH UINT8_C(1) + +/*! BMI3 Interrupt Output Enable */ +#define BMI3_INT_OUTPUT_DISABLE UINT8_C(0) +#define BMI3_INT_OUTPUT_ENABLE UINT8_C(1) + +/******************************************************************************/ +/*! @name Status macros */ +/******************************************************************************/ +#define BMI3_STATUS_POR UINT8_C(0x01) +#define BMI3_STATUS_DRDY_TEMP UINT8_C(0x20) +#define BMI3_STATUS_DRDY_GYR UINT8_C(0x40) +#define BMI3_STATUS_DRDY_ACC UINT8_C(0x80) + +/******************************************************************************/ +/*! @name Gyro self-calibration/self-test coefficient macros */ +/******************************************************************************/ +#define BMI3_SC_ST_VALUE_0 UINT16_C(0x5A2E) +#define BMI3_SC_ST_VALUE_1 UINT16_C(0x9219) +#define BMI3_SC_ST_VALUE_2 UINT16_C(0x5637) +#define BMI3_SC_ST_VALUE_3 UINT16_C(0xFFE8) +#define BMI3_SC_ST_VALUE_4 UINT16_C(0xFFEF) +#define BMI3_SC_ST_VALUE_5 UINT16_C(0x000D) +#define BMI3_SC_ST_VALUE_6 UINT16_C(0x07CA) +#define BMI3_SC_ST_VALUE_7 UINT16_C(0xFFCD) +#define BMI3_SC_ST_VALUE_8 UINT16_C(0xEF6C) + +#define BMI3_SC_SENSITIVITY_EN UINT8_C(1) +#define BMI3_SC_OFFSET_EN UINT8_C(2) + +/*! Self-calibration enable disable macros */ +#define BMI3_SC_APPLY_CORR_DIS UINT8_C(0) +#define BMI3_SC_APPLY_CORR_EN UINT8_C(4) + +/* CMD DATA*/ +#define BMI323_CMD_SOFT_RESET UINT16_C(0xDEAF) +#define BMI323_CMD_SELFTEST UINT16_C(0x0100) +#define BMI323_CMD_GYRO_SELF_CALI UINT16_C(0x0101) +#define BMI323_CMD_GYRO_CALI_ABORT UINT16_C(0x200) +#define BMI323_CMD_I3C_SYNC_CONF UINT16_C(0x201) +#define BMI323_CMD_AXIS_MAP UINT16_C(0x300) + +uint8_t bmi323_detect(); +void bmi323_configure(); + +void bmi3_write8(uint8_t reg, uint8_t data, uint32_t delay); +void bmi3_write16(uint8_t reg, uint16_t data, uint32_t delay); +void bmi3_write_data(uint8_t reg, uint8_t *data, uint32_t size, uint32_t delay); + +uint8_t bmi3_read8(uint8_t reg); +uint16_t bmi3_read16(uint8_t reg); +void bmi323_read_data(uint8_t reg, uint8_t *data, uint32_t size); + +void bmi323_read_gyro_data(gyro_data_t *data); \ No newline at end of file diff --git a/src/driver/spi_gyro.c b/src/driver/spi_gyro.c index 7d2532558..516edb4a6 100644 --- a/src/driver/spi_gyro.c +++ b/src/driver/spi_gyro.c @@ -7,6 +7,7 @@ #include "driver/spi_bmi270.h" #include "driver/spi_icm42605.h" #include "driver/spi_mpu6xxx.h" +#include "driver/spi_bmi323.h" #ifdef USE_GYRO @@ -47,7 +48,12 @@ static gyro_types_t gyro_spi_detect() { break; } // FALLTHROUGH - + case GYRO_TYPE_BMI323: + type = bmi323_detect(); + if (type != GYRO_TYPE_INVALID) { + break; + } + // FALLTHROUGH default: break; } @@ -94,6 +100,10 @@ uint8_t gyro_spi_init() { case GYRO_TYPE_BMI270: bmi270_configure(); break; + case GYRO_TYPE_BMI323: + bmi323_configure(); + break; + default: break; @@ -126,6 +136,10 @@ gyro_data_t gyro_spi_read() { bmi270_read_gyro_data(&data); break; } + case GYRO_TYPE_BMI323: { + bmi323_read_gyro_data(&data); + break; + } default: break; diff --git a/src/driver/spi_gyro.h b/src/driver/spi_gyro.h index da9faf980..d372ed7b4 100644 --- a/src/driver/spi_gyro.h +++ b/src/driver/spi_gyro.h @@ -17,6 +17,7 @@ typedef enum { GYRO_TYPE_ICM42688P, GYRO_TYPE_BMI270, + GYRO_TYPE_BMI323, } gyro_types_t; typedef struct {