diff --git a/make/source.mk b/make/source.mk index ad1a5d0f7a..dd602fab3c 100644 --- a/make/source.mk +++ b/make/source.mk @@ -198,6 +198,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_bmi160.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_icm20689.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 2c17f4b2f5..0bcf72738c 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -33,6 +33,9 @@ #include #endif +#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors +#define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensor + #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE #endif @@ -40,7 +43,7 @@ #define GYRO_HARDWARE_LPF_NORMAL 0 #define GYRO_HARDWARE_LPF_1KHZ_SAMPLE 2 -#if defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) #define GYRO_HARDWARE_LPF_EXPERIMENTAL 3 #else #define GYRO_HARDWARE_LPF_EXPERIMENTAL 1 diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 5265e27342..f4b913736e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -53,6 +53,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -404,6 +405,19 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { gyro->mpuDetectionResult.sensor = sensor; return true; } +#endif +#ifdef USE_ACCGYRO_BMI270 +#ifndef USE_DUAL_GYRO + spiBusSetInstance(&gyro->bus, BMI270_SPI_INSTANCE); +#endif +#ifdef BMI270_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI270_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = bmi270Detect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + return true; + } #endif return false; } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index aa0f770e29..b85e6d4ca4 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -29,7 +29,7 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_USES_SPI #endif @@ -219,6 +219,7 @@ typedef enum { ICM_42605_SPI, ICM_42688P_SPI, BMI_160_SPI, + BMI_270_SPI, IMUF_9001_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c new file mode 100644 index 0000000000..b7e4749770 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -0,0 +1,499 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_BMI270 + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +// 10 MHz max SPI frequency +#define BMI270_MAX_SPI_CLK_HZ 10000000 + +#define BMI270_FIFO_FRAME_SIZE 6 + +#define BMI270_CONFIG_SIZE 328 + +// Declaration for the device config (microcode) that must be uploaded to the sensor +const uint8_t bmi270_maximum_fifo_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f +}; + +#define BMI270_CHIP_ID 0x24 + +// BMI270 registers (not the complete list) +typedef enum { + BMI270_REG_CHIP_ID = 0x00, + BMI270_REG_ERR_REG = 0x02, + BMI270_REG_STATUS = 0x03, + BMI270_REG_ACC_DATA_X_LSB = 0x0C, + BMI270_REG_GYR_DATA_X_LSB = 0x12, + BMI270_REG_SENSORTIME_0 = 0x18, + BMI270_REG_SENSORTIME_1 = 0x19, + BMI270_REG_SENSORTIME_2 = 0x1A, + BMI270_REG_EVENT = 0x1B, + BMI270_REG_INT_STATUS_0 = 0x1C, + BMI270_REG_INT_STATUS_1 = 0x1D, + BMI270_REG_INTERNAL_STATUS = 0x21, + BMI270_REG_TEMPERATURE_LSB = 0x22, + BMI270_REG_TEMPERATURE_MSB = 0x23, + BMI270_REG_FIFO_LENGTH_LSB = 0x24, + BMI270_REG_FIFO_LENGTH_MSB = 0x25, + BMI270_REG_FIFO_DATA = 0x26, + BMI270_REG_ACC_CONF = 0x40, + BMI270_REG_ACC_RANGE = 0x41, + BMI270_REG_GYRO_CONF = 0x42, + BMI270_REG_GYRO_RANGE = 0x43, + BMI270_REG_AUX_CONF = 0x44, + BMI270_REG_FIFO_DOWNS = 0x45, + BMI270_REG_FIFO_WTM_0 = 0x46, + BMI270_REG_FIFO_WTM_1 = 0x47, + BMI270_REG_FIFO_CONFIG_0 = 0x48, + BMI270_REG_FIFO_CONFIG_1 = 0x49, + BMI270_REG_SATURATION = 0x4A, + BMI270_REG_INT1_IO_CTRL = 0x53, + BMI270_REG_INT2_IO_CTRL = 0x54, + BMI270_REG_INT_LATCH = 0x55, + BMI270_REG_INT1_MAP_FEAT = 0x56, + BMI270_REG_INT2_MAP_FEAT = 0x57, + BMI270_REG_INT_MAP_DATA = 0x58, + BMI270_REG_INIT_CTRL = 0x59, + BMI270_REG_INIT_DATA = 0x5E, + BMI270_REG_ACC_SELF_TEST = 0x6D, + BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, + BMI270_REG_PWR_CONF = 0x7C, + BMI270_REG_PWR_CTRL = 0x7D, + BMI270_REG_CMD = 0x7E, +} bmi270Register_e; + +// BMI270 register configuration values +typedef enum { + BMI270_VAL_CMD_SOFTRESET = 0xB6, + BMI270_VAL_CMD_FIFOFLUSH = 0xB0, + BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors + BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake + BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz + BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz + BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode + BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode + BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale + BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale + BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz + BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00, // set gyro filter in OSR4 mode + BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01, // set gyro filter in OSR2 mode + BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode + BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode + + BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale + // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well + // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) + + BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 + BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 + BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled + BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame + BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode + BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) + BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) + BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB +} bmi270ConfigValues_e; + +// BMI270 register reads are 16bits with the first byte a "dummy" value 0 +// that must be ignored. The result is in the second byte. +static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId) +{ + uint8_t data[2] = { 0, 0 }; + + if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) { + return data[1]; + } else { + return 0; + } +} + +static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +{ + spiBusWriteRegister(bus, registerId, value); + if (delayMs) { + delay(delayMs); + } +} + +// Toggle the CS to switch the device into SPI mode. +// Device switches initializes as I2C and switches to SPI on a low to high CS transition +static void bmi270EnableSPI(const busDevice_t *bus) +{ + IOLo(bus->busdev_u.spi.csnPin); + delay(1); + IOHi(bus->busdev_u.spi.csnPin); + delay(10); +} + +uint8_t bmi270Detect(const busDevice_t *bus) +{ +#ifndef USE_DUAL_GYRO + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); +#endif + + spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(bus); + + if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + return BMI_270_SPI; + } + + return MPU_NONE; +} + +static void bmi270UploadConfig(const busDevice_t *bus) +{ + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1); + + // Transfer the config file + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA); + spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file)); + IOHi(bus->busdev_u.spi.csnPin); + + delay(10); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); +} + +static uint8_t getBmiOsrMode() +{ + switch(gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR4; + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR2; + } + return 0; +} + +static void bmi270Config(const gyroDev_t *gyro) +{ + const busDevice_t *bus = &gyro->bus; + + // If running in hardware_lpf experimental mode then switch to FIFO-based, + // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); +#else + const bool fifoMode = false; +#endif + + // Perform a soft reset to set all configuration to default + // Delay 100ms before continuing configuration + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + + // Toggle the chip into SPI mode + bmi270EnableSPI(bus); + + bmi270UploadConfig(bus); + + // Configure the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + } + + // Configure the accelerometer + bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + + // Configure the accelerometer full-scale range + bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + + // Configure the gyro + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + + // Configure the gyro full-range scale + bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + + // Configure the gyro data ready interrupt + if (fifoMode) { + // Interrupt driven by FIFO watermark level + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + } else { + // Interrupt driven by data ready + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + } + + // Configure the behavior of the INT1 pin + bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + + // Configure the device for performance mode + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + + // Enable the gyro, accelerometer and temperature sensor - disable aux interface + bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + + // Flush the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + } +} + +extiCallbackRec_t bmi270IntCallbackRec; + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} + +static void bmi270IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); + EXTIEnable(mpuIntIO, true); +} +#endif + +static bool bmi270AccRead(accDev_t *acc) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(acc->bus.busdev_u.spi.csnPin); + spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(acc->bus.busdev_u.spi.csnPin); + + acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); + + return true; +} + +static bool bmi270GyroReadRegister(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + return true; +} + +#ifdef USE_GYRO_DLPF_EXPERIMENTAL +static bool bmi270GyroReadFifo(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_FIFO_LENGTH_L, + IDX_FIFO_LENGTH_H, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + bool dataRead = false; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + + // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for + // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the + // length before using the sample. + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); + + if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { + + const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). + // This shouldn't happen since we're only using the data if the FIFO length indicates + // that data is available, but this safeguard is needed to prevent bad things in + // case it does happen. + if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { + gyro->gyroADCRaw[X] = gyroX; + gyro->gyroADCRaw[Y] = gyroY; + gyro->gyroADCRaw[Z] = gyroZ; + dataRead = true; + } + fifoLength -= BMI270_FIFO_FRAME_SIZE; + } + + // If there are additional samples in the FIFO then we don't use those for now and simply + // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since + // the gyro loop is running at the native sample rate of 6.4KHz. + // However the way the FIFO works in the sensor is that if a frame is partially read then + // it remains in the queue instead of bein removed. So if we ever got into a state where there + // was a partial frame or other unexpected data in the FIFO is may never get cleared and we + // would end up in a lock state of always re-reading the same partial or invalid sample. + if (fifoLength > 0) { + // Partial or additional frames left - flush the FIFO + bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + } + + return dataRead; +} +#endif + +static bool bmi270GyroRead(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // running in 6.4KHz FIFO mode + return bmi270GyroReadFifo(gyro); + } else +#endif + { + // running in 3.2KHz register mode + return bmi270GyroReadRegister(gyro); + } +} + +static void bmi270SpiGyroInit(gyroDev_t *gyro) +{ + bmi270Config(gyro); + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) + bmi270IntExtiInit(gyro); +#endif +} + +static void bmi270SpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool bmi270SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + acc->initFn = bmi270SpiAccInit; + acc->readFn = bmi270AccRead; + + return true; +} + + +bool bmi270SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + gyro->initFn = bmi270SpiGyroInit; + gyro->readFn = bmi270GyroRead; + gyro->scale = GYRO_SCALE_2000DPS; + + return true; +} + +// Used to query the status register to determine what event caused the EXTI to fire. +// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready +// trigger will fire for both gyro and accelerometer. So it's necessary to check this register +// to determine which event caused the interrupt. +// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. +// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO +// watermark reason as an idication of gyro data ready. +uint8_t bmi270InterruptStatus(gyroDev_t *gyro) +{ + return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1); +} +#endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.h b/src/main/drivers/accgyro/accgyro_spi_bmi270.h new file mode 100644 index 0000000000..c0ea0495fa --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" + +uint8_t bmi270Detect(const busDevice_t *bus); +bool bmi270SpiAccDetect(accDev_t *acc); +bool bmi270SpiGyroDetect(gyroDev_t *gyro); +uint8_t bmi270InterruptStatus(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index bc9ad37f27..3653c24d43 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -53,16 +53,24 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin } gyro->mpuDividerDrops = gyroSyncDenominator - 1; gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_8_kHz : GYRO_RATE_1_kHz; - //20649 is a weird gyro - if (gyro->mpuDetectionResult.sensor == ICM_20649_SPI) { - gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; - } else if (gyro->mpuDetectionResult.sensor == BMI_160_SPI && lpfNoneOr256) { - //brainFPV is also a weird gyro - gyro->gyroRateKHz = GYRO_RATE_3200_Hz; - } else if (gyro_use_32khz) { - //use full 32k - gyro->gyroRateKHz = GYRO_RATE_32_kHz; + + switch (gyro->mpuDetectionResult.sensor) { + case ICM_20649_SPI: //20649 is a weird gyro + gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; + break; + case BMI_160_SPI: //brainFPV is also a weird gyro + if (lpfNoneOr256) { gyro->gyroRateKHz = GYRO_RATE_3200_Hz; } + break; + case BMI_270_SPI: //bmi270 + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + break; + default: + if (gyro_use_32khz) { + //use full 32k + gyro->gyroRateKHz = GYRO_RATE_32_kHz; + } } + // return the targetLooptime (expected cycleTime) return (uint32_t)(gyroSyncDenominator * gyro->gyroRateKHz); } diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 6bddbb9c5b..71a62b210b 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -252,6 +252,7 @@ void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { bus->busdev_u.spi.instance = instance; } +// icm42688p and bmi270 porting uint16_t spiCalculateDivider(uint32_t freq) { #if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5ccab8f8b5..37ca42323d 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -434,6 +434,7 @@ void validateAndFixGyroConfig(void) { samplingTime = 1.0f / 9000.0f; break; case BMI_160_SPI: + case BMI_270_SPI: samplingTime = 0.0003125f; break; default: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 231c6fa489..be511b02f7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -86,7 +86,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #ifdef STM32F10X #define PID_PROCESS_DENOM_DEFAULT 1 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define PID_PROCESS_DENOM_DEFAULT 1 #else #define PID_PROCESS_DENOM_DEFAULT 2 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9af64f98e5..9cedcd3a45 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -109,13 +109,13 @@ const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "ACC_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "ACC_IMUF9001", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "GYRO_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "GYRO_IMUF9001", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) @@ -252,10 +252,10 @@ static const char * const lookupTableRxSpi[] = { static const char * const lookupTableGyroHardwareLpf[] = { "NORMAL", -#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) "OPTION1", "OPTION2", -#if defined(USE_GYRO_SPI_ICM42688P) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // will need bmi270 logic as well +#if ( defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) ) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // icm42688p & bmi270 experimental "EXPERIMENTAL", #endif #else diff --git a/src/main/pg/bus_spi.c b/src/main/pg/bus_spi.c index 3b1fcfa7a8..e8e50fc84f 100644 --- a/src/main/pg/bus_spi.c +++ b/src/main/pg/bus_spi.c @@ -100,6 +100,9 @@ ioTag_t preinitIPUList[SPI_PREINIT_IPU_COUNT] = { #ifdef BMI160_CS_PIN IO_TAG(BMI160_CS_PIN), #endif +#ifdef BMI270_CS_PIN + IO_TAG(BMI270_CS_PIN), +#endif #ifdef L3GD20_CS_PIN IO_TAG(L3GD20_CS_PIN), #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 62460a182c..6a45d357b4 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,6 +43,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -299,6 +300,17 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case ACC_BMI270: + if (bmi270SpiAccDetect(dev)) { + accHardware = ACC_BMI270; +#ifdef ACC_BMI270_ALIGN + dev->accAlign = ACC_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) case ACC_ICM42605: case ACC_ICM42688P: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b448d2419b..5b132c53f6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -50,6 +50,7 @@ typedef enum { ACC_ICM42605, ACC_ICM42688P, ACC_BMI160, + ACC_BMI270, ACC_IMUF9001, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index eefea7e341..9ec6a642fc 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -211,7 +212,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 3 @@ -537,6 +538,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case GYRO_BMI270: + if (bmi270SpiGyroDetect(dev)) { + gyroHardware = GYRO_BMI270; +#ifdef GYRO_BMI270_ALIGN + dev->gyroAlign = GYRO_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #ifdef USE_GYRO_IMUF9001 case GYRO_IMUF9001: if (imufSpiGyroDetect(dev)) { @@ -566,7 +578,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { static bool gyroInitSensor(gyroSensor_t *gyroSensor) { gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif @@ -611,6 +623,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) { case GYRO_MPU3050: case GYRO_L3GD20: case GYRO_BMI160: + case GYRO_BMI270: case GYRO_MPU6000: case GYRO_MPU6500: case GYRO_MPU9250: diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b81475a75c..671d7b7cbd 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -56,6 +56,7 @@ typedef enum { GYRO_ICM42605, GYRO_ICM42688P, GYRO_BMI160, + GYRO_BMI270, GYRO_IMUF9001, GYRO_FAKE } gyroSensor_e; diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 426ec9b767..4d378c10ac 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -229,5 +229,5 @@ #define USE_CMS_GPS_RESCUE_MENU #endif -// ICM42688P define +// ICM42688P & BMI270 experimental define #define USE_GYRO_DLPF_EXPERIMENTAL