From be3be40193300e5e35b4000067455c7e241f6171 Mon Sep 17 00:00:00 2001 From: "nyoman.gunawan" Date: Sat, 9 Sep 2023 18:32:56 +0800 Subject: [PATCH 1/5] add gyro bmi160 and fix gyro i2c address --- README.md | 2 + lib/Espfc/src/Device/BaroBMP280.h | 5 +- lib/Espfc/src/Device/GyroBMI160.h | 175 ++++++++++++++++++++++++++++ lib/Espfc/src/Device/GyroDevice.h | 3 +- lib/Espfc/src/Device/GyroICM20602.h | 7 +- lib/Espfc/src/Device/GyroLSM6DSO.h | 5 +- lib/Espfc/src/Device/GyroMPU6050.h | 7 +- lib/Espfc/src/Device/GyroMPU6500.h | 2 +- lib/Espfc/src/Device/GyroMPU9250.h | 2 +- lib/Espfc/src/Device/MagAK8963.h | 8 +- lib/Espfc/src/Hardware.h | 4 + 11 files changed, 201 insertions(+), 19 deletions(-) create mode 100644 lib/Espfc/src/Device/GyroBMI160.h diff --git a/README.md b/README.md index 68a0d1d5..8d5e0bbd 100644 --- a/README.md +++ b/README.md @@ -93,6 +93,8 @@ After flashing you need to configure few things first: | AK8963/I2C | - | Yes | Yes | | ICM20602/I2C | ? | ? | ? | | ICM20602/SPI | - | Yes | Yes | +| BMI160/I2C | ? | Yes | ? | +| BMI160/SPI | - | Yes | Yes | ? - not tested, but should work diff --git a/lib/Espfc/src/Device/BaroBMP280.h b/lib/Espfc/src/Device/BaroBMP280.h index d680b504..a6a63921 100644 --- a/lib/Espfc/src/Device/BaroBMP280.h +++ b/lib/Espfc/src/Device/BaroBMP280.h @@ -5,7 +5,8 @@ #include "BaroDevice.h" #include "Debug_Espfc.h" -#define BMP280_DEFAULT_ADDRESS 0x77 +#define BMP280_ADDRESS_FIRST 0x76 +#define BMP280_ADDRESS_SECOND 0x77 #define BMP280_WHOAMI_ID 0x58 #define BMP280_WHOAMI_REG 0xD0 @@ -60,7 +61,7 @@ class BaroBMP280: public BaroDevice int begin(BusDevice * bus) override { - return begin(bus, BMP280_DEFAULT_ADDRESS); + return begin(bus, BMP280_ADDRESS_FIRST) ? 1 : begin(bus, BMP280_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/GyroBMI160.h b/lib/Espfc/src/Device/GyroBMI160.h new file mode 100644 index 00000000..469d3a69 --- /dev/null +++ b/lib/Espfc/src/Device/GyroBMI160.h @@ -0,0 +1,175 @@ +#ifndef _ESPFC_DEVICE_GYRO_BMI160_H_ +#define _ESPFC_DEVICE_GYRO_BMI160_H_ + +#include "BusDevice.h" +#include "GyroDevice.h" +#include "helper_3dmath.h" +#include "Debug_Espfc.h" + +#define BMI160_ADDRESS_FIRST 0x69 +#define BMI160_ADDRESS_SECOND 0x68 +#define BMI160_RA_CHIP_ID 0x00 +#define BMI160_CHIP_ID_DEFAULT_VALUE 0xD1 + +#define BMI160_RA_GYRO_X_L 0x0C +#define BMI160_RA_GYRO_X_H 0x0D +#define BMI160_RA_GYRO_Y_L 0x0E +#define BMI160_RA_GYRO_Y_H 0x0F +#define BMI160_RA_GYRO_Z_L 0x10 +#define BMI160_RA_GYRO_Z_H 0x11 +#define BMI160_RA_ACCEL_X_L 0x12 +#define BMI160_RA_ACCEL_X_H 0x13 +#define BMI160_RA_ACCEL_Y_L 0x14 +#define BMI160_RA_ACCEL_Y_H 0x15 +#define BMI160_RA_ACCEL_Z_L 0x16 +#define BMI160_RA_ACCEL_Z_H 0x17 + +#define BMI160_RA_ACCEL_CONF 0X40 +#define BMI160_RA_ACCEL_RANGE 0X41 + +#define BMI160_RA_GYRO_CONF 0X42 +#define BMI160_RA_GYRO_RANGE 0X43 + +#define BMI160_CMD_START_FOC 0x03 +#define BMI160_CMD_ACC_MODE_NORMAL 0x11 +#define BMI160_CMD_GYR_MODE_NORMAL 0x15 +#define BMI160_CMD_FIFO_FLUSH 0xB0 +#define BMI160_CMD_INT_RESET 0xB1 +#define BMI160_CMD_STEP_CNT_CLR 0xB2 +#define BMI160_CMD_SOFT_RESET 0xB6 + +#define BMI160_RA_CMD 0x7E + +namespace Espfc { + +namespace Device { + +class GyroBMI160: public GyroDevice +{ + public: + int begin(BusDevice * bus) override + { + return begin(bus, BMI160_ADDRESS_FIRST) ? 1 : begin(bus, BMI160_ADDRESS_SECOND) ? 1 : 0; + } + + int begin(BusDevice * bus, uint8_t addr) override + { + setBus(bus, addr); + + if(!testConnection()) return 0; + + // reset device + _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); + delay(1); + + /* Issue a dummy-read to force the device into SPI comms mode */ + uint8_t dummy = 0; + _bus->readByte(_addr, 0x7F, &dummy); + delay(1); + + // Start up accelerometer + _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL); + delay(100); + + // Start up gyroscope + _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL); + delay(100); + + // // Set odr and ranges + // // Set acc_us = 0 & acc_bwp = 0b001 for high performance and OSR2 mode + // _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, 0x00 | 0x10 | 0x0B); + // delay(1); + + // // Set up full scale Accel range. +-16G + _bus->writeByte(_addr, BMI160_RA_ACCEL_RANGE, 0x0C); + delay(1); + + // // Set up full scale Gyro range. +-2000dps + _bus->writeByte(_addr, BMI160_RA_GYRO_RANGE, 0x00); + delay(1); + + // Set Accel ODR to 400hz, BWP mode to Oversample 4, LPF of ~40.5hz + _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, 0x0A); + delay(1); + + // Set Gyro ODR to 400hz, BWP mode to Oversample 4, LPF of ~34.15hz + _bus->writeByte(_addr, BMI160_RA_GYRO_CONF, 0x0A); + delay(1); + + return 1; + } + + GyroDeviceType getType() const override + { + return GYRO_BMI160; + } + + int readGyro(VectorInt16& v) override + { + uint8_t buffer[6]; + + _bus->readFast(_addr, BMI160_RA_GYRO_X_L, 6, buffer); + + v.x = (((int16_t)buffer[1]) << 8) | buffer[0]; + v.y = (((int16_t)buffer[3]) << 8) | buffer[2]; + v.z = (((int16_t)buffer[5]) << 8) | buffer[4]; + + return 1; + } + + int readAccel(VectorInt16& v) override + { + uint8_t buffer[6]; + + _bus->readFast(_addr, BMI160_RA_ACCEL_X_L, 6, buffer); + + v.x = (((int16_t)buffer[1]) << 8) | buffer[0]; + v.y = (((int16_t)buffer[3]) << 8) | buffer[2]; + v.z = (((int16_t)buffer[5]) << 8) | buffer[4]; + + return 1; + } + + void setDLPFMode(uint8_t mode) override + { + } + + int getRate() const override + { + return 8000; + } + + void setRate(int rate) override + { + } + + void setFullScaleGyroRange(uint8_t range) override + { + } + + void setFullScaleAccelRange(uint8_t range) override + { + } + + bool testConnection() override + { + uint8_t whoami = 0; + _bus->readByte(_addr, BMI160_RA_CHIP_ID, &whoami); + //D("bmi160:whoami", _addr, whoami); + return whoami == BMI160_CHIP_ID_DEFAULT_VALUE; + } + + void setSleepEnabled(bool enabled) + { + } + + void setClockSource(uint8_t source) + { + } +}; + +} + +} + +#endif diff --git a/lib/Espfc/src/Device/GyroDevice.h b/lib/Espfc/src/Device/GyroDevice.h index 774312e1..47a7993f 100644 --- a/lib/Espfc/src/Device/GyroDevice.h +++ b/lib/Espfc/src/Device/GyroDevice.h @@ -16,6 +16,7 @@ enum GyroDeviceType { GYRO_MPU9250 = 5, GYRO_LSM6DSO = 6, GYRO_ICM20602 = 7, + GYRO_BMI160 = 8, GYRO_MAX }; @@ -44,7 +45,7 @@ class GyroDevice: public BusAwareDevice static const char ** getNames() { - static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"), NULL }; + static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL }; return devChoices; } diff --git a/lib/Espfc/src/Device/GyroICM20602.h b/lib/Espfc/src/Device/GyroICM20602.h index 55b0b952..462e4017 100644 --- a/lib/Espfc/src/Device/GyroICM20602.h +++ b/lib/Espfc/src/Device/GyroICM20602.h @@ -6,9 +6,8 @@ #include "helper_3dmath.h" #include "Debug_Espfc.h" -#define ICM20602_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board -#define ICM20602_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define ICM20602_DEFAULT_ADDRESS ICM20602_ADDRESS_AD0_LOW +#define ICM20602_ADDRESS_FIRST 0x68 // address pin low (GND), default for InvenSense evaluation board +#define ICM20602_ADDRESS_SECOND 0x69 // address pin high (VCC) #define ICM20602_RA_SMPLRT_DIV 0x19 #define ICM20602_RA_CONFIG 0x1A @@ -87,7 +86,7 @@ class GyroICM20602: public GyroDevice public: int begin(BusDevice * bus) override { - return begin(bus, ICM20602_DEFAULT_ADDRESS); + return begin(bus, ICM20602_ADDRESS_FIRST) ? 1 : begin(bus, ICM20602_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/GyroLSM6DSO.h b/lib/Espfc/src/Device/GyroLSM6DSO.h index 0ed516c9..66577607 100644 --- a/lib/Espfc/src/Device/GyroLSM6DSO.h +++ b/lib/Espfc/src/Device/GyroLSM6DSO.h @@ -7,7 +7,8 @@ #include "Debug_Espfc.h" // https://github.com/arduino-libraries/Arduino_LSM6DSOX/blob/master/src/LSM6DSOX.cpp -#define LSM6DSOX_DEFAULT_ADDRESS 0x6A +#define LSM6DSOX_ADDRESS_FIRST 0x6A +#define LSM6DSOX_ADDRESS_SECOND 0x6b // registers #define LSM6DSO_REG_WHO_AM_I 0x0F @@ -69,7 +70,7 @@ class GyroLSM6DSO: public GyroDevice public: int begin(BusDevice * bus) override { - return begin(bus, LSM6DSOX_DEFAULT_ADDRESS); + return begin(bus, LSM6DSOX_ADDRESS_FIRST) ? 1 : begin(bus, LSM6DSOX_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/GyroMPU6050.h b/lib/Espfc/src/Device/GyroMPU6050.h index 0af80552..9d2615d7 100644 --- a/lib/Espfc/src/Device/GyroMPU6050.h +++ b/lib/Espfc/src/Device/GyroMPU6050.h @@ -6,9 +6,8 @@ #include "helper_3dmath.h" #include "Debug_Espfc.h" -#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board -#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW +#define MPU6050_ADDRESS_FIRST 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_SECOND 0x69 // address pin high (VCC) #define MPU6050_RA_SMPLRT_DIV 0x19 #define MPU6050_RA_CONFIG 0x1A @@ -87,7 +86,7 @@ class GyroMPU6050: public GyroDevice public: int begin(BusDevice * bus) override { - return begin(bus, MPU6050_DEFAULT_ADDRESS); + return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/GyroMPU6500.h b/lib/Espfc/src/Device/GyroMPU6500.h index 20adb40f..f899dc7a 100644 --- a/lib/Espfc/src/Device/GyroMPU6500.h +++ b/lib/Espfc/src/Device/GyroMPU6500.h @@ -27,7 +27,7 @@ class GyroMPU6500: public GyroMPU6050 public: int begin(BusDevice * bus) override { - return begin(bus, MPU6050_DEFAULT_ADDRESS); + return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/GyroMPU9250.h b/lib/Espfc/src/Device/GyroMPU9250.h index b94c6f81..d9112710 100644 --- a/lib/Espfc/src/Device/GyroMPU9250.h +++ b/lib/Espfc/src/Device/GyroMPU9250.h @@ -25,7 +25,7 @@ class GyroMPU9250: public GyroMPU6050 public: int begin(BusDevice * bus) override { - return begin(bus, MPU6050_DEFAULT_ADDRESS); + return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override diff --git a/lib/Espfc/src/Device/MagAK8963.h b/lib/Espfc/src/Device/MagAK8963.h index c85dcb38..7a9324f9 100644 --- a/lib/Espfc/src/Device/MagAK8963.h +++ b/lib/Espfc/src/Device/MagAK8963.h @@ -4,8 +4,8 @@ #include "MagDevice.h" #include "BusDevice.h" -#define MPU9250_ADDRESS 0x68 -#define MPU9250_DEFAULT_ADDRESS MPU9250_ADDRESS +#define MPU9250_ADDRESS_FIRST 0x68 +#define MPU9250_ADDRESS_SECOND 0x68 #define MPU9250_I2C_SLV0_ADDR 0x25 #define MPU9250_I2C_SLV0_REG 0x26 #define MPU9250_I2C_SLV0_DO 0x63 @@ -37,14 +37,14 @@ class MagAK8963: public MagDevice public: int begin(BusDevice * bus) override { - return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_DEFAULT_ADDRESS); + return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_FIRST) ? 1 : begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_SECOND) ? 1 : 0; } int begin(BusDevice * bus, uint8_t addr) override { if(bus->getType() == BUS_I2C) { - return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_DEFAULT_ADDRESS); + return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_FIRST) ? 1 : begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_SECOND) ? 1 : 0; } else { diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index c9a97bf7..e65accab 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -17,6 +17,7 @@ #include "Device/GyroMPU9250.h" #include "Device/GyroLSM6DSO.h" #include "Device/GyroICM20602.h" +#include "Device/GyroBMI160.h" #include "Device/MagHMC5338L.h" #include "Device/MagAK8963.h" #include "Device/BaroDevice.h" @@ -38,6 +39,7 @@ namespace { static Espfc::Device::GyroMPU9250 mpu9250; static Espfc::Device::GyroLSM6DSO lsm6dso; static Espfc::Device::GyroICM20602 icm20602; + static Espfc::Device::GyroBMI160 bmi160; static Espfc::Device::MagHMC5338L hmc5883l; static Espfc::Device::MagAK8963 ak8963; static Espfc::Device::BaroBMP085 bmp085; @@ -92,6 +94,7 @@ class Hardware if(!detectedGyro && detectDevice(mpu9250, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu9250; if(!detectedGyro && detectDevice(mpu6500, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu6500; if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602; + if(!detectedGyro && detectDevice(bmi160, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &bmi160; if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso; } #endif @@ -101,6 +104,7 @@ class Hardware if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250; if(!detectedGyro && detectDevice(mpu6500, i2cBus)) detectedGyro = &mpu6500; if(!detectedGyro && detectDevice(icm20602, i2cBus)) detectedGyro = &icm20602; + if(!detectedGyro && detectDevice(bmi160, i2cBus)) detectedGyro = &bmi160; if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050; if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso; } From 5cf3322621442782e95551683ba59a3d45535b2c Mon Sep 17 00:00:00 2001 From: "nyoman.gunawan" Date: Thu, 14 Sep 2023 03:56:11 +0800 Subject: [PATCH 2/5] fix bmi160 gyro rate --- lib/Espfc/src/Device/BusDevice.h | 29 ++++++ lib/Espfc/src/Device/GyroBMI160.h | 155 ++++++++++++++++++++++++++---- 2 files changed, 165 insertions(+), 19 deletions(-) diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h index bb54711c..3738d263 100644 --- a/lib/Espfc/src/Device/BusDevice.h +++ b/lib/Espfc/src/Device/BusDevice.h @@ -72,6 +72,19 @@ class BusDevice return count; } + int8_t readBitsBMI160(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) + { + uint8_t count, b; + if ((count = readByte(devAddr, regAddr, &b)) != 0) + { + uint8_t mask = (1 << length) - 1; + b >>= bitStart; + b &= mask; + *data = b; + } + return count; + } + bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { uint8_t b; @@ -96,6 +109,22 @@ class BusDevice } } + bool writeBitsBMI160(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) + { + uint8_t b = 0; + if (readByte(devAddr, regAddr, &b) != 0) + { + uint8_t mask = ((1 << length) - 1) << bitStart; + data <<= bitStart; // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return writeByte(devAddr, regAddr, b); + } else { + return false; + } + } + bool writeMask(uint8_t devAddr, uint8_t regAddr, uint8_t mask, uint8_t data) { uint8_t b = 0; diff --git a/lib/Espfc/src/Device/GyroBMI160.h b/lib/Espfc/src/Device/GyroBMI160.h index 469d3a69..25f4792f 100644 --- a/lib/Espfc/src/Device/GyroBMI160.h +++ b/lib/Espfc/src/Device/GyroBMI160.h @@ -6,8 +6,8 @@ #include "helper_3dmath.h" #include "Debug_Espfc.h" -#define BMI160_ADDRESS_FIRST 0x69 -#define BMI160_ADDRESS_SECOND 0x68 +#define BMI160_ADDRESS_FIRST 0x69 +#define BMI160_ADDRESS_SECOND 0x68 #define BMI160_RA_CHIP_ID 0x00 #define BMI160_CHIP_ID_DEFAULT_VALUE 0xD1 @@ -24,12 +24,38 @@ #define BMI160_RA_ACCEL_Z_L 0x16 #define BMI160_RA_ACCEL_Z_H 0x17 +#define BMI160_ACCEL_RATE_SEL_BIT 0 +#define BMI160_ACCEL_RATE_SEL_LEN 4 + #define BMI160_RA_ACCEL_CONF 0X40 #define BMI160_RA_ACCEL_RANGE 0X41 +#define BMI160_GYRO_RATE_SEL_BIT 0 +#define BMI160_GYRO_RATE_SEL_LEN 4 + #define BMI160_RA_GYRO_CONF 0X42 #define BMI160_RA_GYRO_RANGE 0X43 +#define BMI160_ACC_OFFSET_EN 6 +#define BMI160_ACC_OFFSET_LEN 1 +#define BMI160_GYR_OFFSET_EN 7 +#define BMI160_GYR_OFFSET_LEN 1 + +#define BMI160_RA_OFFSET_0 0x71 +#define BMI160_RA_OFFSET_1 0x72 +#define BMI160_RA_OFFSET_2 0x73 +#define BMI160_RA_OFFSET_3 0x74 +#define BMI160_RA_OFFSET_4 0x75 +#define BMI160_RA_OFFSET_5 0x76 +#define BMI160_RA_OFFSET_6 0x77 + +#define BMI160_REG_INT_EN1 0x51 +#define BMI160_INT_EN1_DRDY 0x10 +#define BMI160_REG_INT_OUT_CTRL 0x53 +#define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A +#define BMI160_REG_INT_MAP1 0x56 +#define BMI160_REG_INT_MAP1_INT1_DRDY 0x80 + #define BMI160_CMD_START_FOC 0x03 #define BMI160_CMD_ACC_MODE_NORMAL 0x11 #define BMI160_CMD_GYR_MODE_NORMAL 0x15 @@ -37,6 +63,8 @@ #define BMI160_CMD_INT_RESET 0xB1 #define BMI160_CMD_STEP_CNT_CLR 0xB2 #define BMI160_CMD_SOFT_RESET 0xB6 +#define BMI160_CMD_SPI_MODE 0x7F +#define BMI160_RESULT_OK 0x1 #define BMI160_RA_CMD 0x7E @@ -47,6 +75,43 @@ namespace Device { class GyroBMI160: public GyroDevice { public: + enum { + BMI160_ACCEL_RANGE_2G = 0X03, /**< +/- 2g range */ + BMI160_ACCEL_RANGE_4G = 0X05, /**< +/- 4g range */ + BMI160_ACCEL_RANGE_8G = 0X08, /**< +/- 8g range */ + BMI160_ACCEL_RANGE_16G = 0X0C, /**< +/- 16g range */ + }; + + enum { + BMI160_GYRO_RANGE_2000 = 0, /**< +/- 2000 degrees/second */ + BMI160_GYRO_RANGE_1000, /**< +/- 1000 degrees/second */ + BMI160_GYRO_RANGE_500, /**< +/- 500 degrees/second */ + BMI160_GYRO_RANGE_250, /**< +/- 250 degrees/second */ + BMI160_GYRO_RANGE_125, /**< +/- 125 degrees/second */ + }; + + enum { + BMI160_ACCEL_RATE_25_2HZ = 5, /**< 25/2 Hz */ + BMI160_ACCEL_RATE_25HZ, /**< 25 Hz */ + BMI160_ACCEL_RATE_50HZ, /**< 50 Hz */ + BMI160_ACCEL_RATE_100HZ, /**< 100 Hz */ + BMI160_ACCEL_RATE_200HZ, /**< 200 Hz */ + BMI160_ACCEL_RATE_400HZ, /**< 400 Hz */ + BMI160_ACCEL_RATE_800HZ, /**< 800 Hz */ + BMI160_ACCEL_RATE_1600HZ, /**< 1600 Hz */ + }; + + enum { + BMI160_GYRO_RATE_25HZ = 6, /**< 25 Hz */ + BMI160_GYRO_RATE_50HZ, /**< 50 Hz */ + BMI160_GYRO_RATE_100HZ, /**< 100 Hz */ + BMI160_GYRO_RATE_200HZ, /**< 200 Hz */ + BMI160_GYRO_RATE_400HZ, /**< 400 Hz */ + BMI160_GYRO_RATE_800HZ, /**< 800 Hz */ + BMI160_GYRO_RATE_1600HZ, /**< 1600 Hz */ + BMI160_GYRO_RATE_3200HZ, /**< 3200 Hz */ + }; + int begin(BusDevice * bus) override { return begin(bus, BMI160_ADDRESS_FIRST) ? 1 : begin(bus, BMI160_ADDRESS_SECOND) ? 1 : 0; @@ -62,10 +127,16 @@ class GyroBMI160: public GyroDevice _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); delay(1); - /* Issue a dummy-read to force the device into SPI comms mode */ - uint8_t dummy = 0; - _bus->readByte(_addr, 0x7F, &dummy); - delay(1); + if(_bus->isSPI()) + { + /* + Issue a dummy-read to force the device into SPI comms mode + see https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi160-ds000.pdf section 3.2.1 + */ + uint8_t dummy = 0; + _bus->readByte(_addr, BMI160_CMD_SPI_MODE, &dummy); + delay(100); + } // Start up accelerometer _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL); @@ -75,25 +146,40 @@ class GyroBMI160: public GyroDevice _bus->writeByte(_addr, BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL); delay(100); - // // Set odr and ranges - // // Set acc_us = 0 & acc_bwp = 0b001 for high performance and OSR2 mode - // _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, 0x00 | 0x10 | 0x0B); - // delay(1); + // Set up full scale Accel range. +-16G + _bus->writeByte(_addr, BMI160_RA_ACCEL_RANGE, BMI160_ACCEL_RANGE_16G); + delay(1); + + // Set up full scale Gyro range. +-2000dps + _bus->writeByte(_addr, BMI160_RA_GYRO_RANGE, BMI160_GYRO_RANGE_2000); + delay(1); + + // Enable accel offset + _bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_ACC_OFFSET_EN, BMI160_ACC_OFFSET_LEN, BMI160_RESULT_OK); + delay(1); + + // Enable gyro offset + _bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_GYR_OFFSET_EN, BMI160_GYR_OFFSET_LEN, BMI160_RESULT_OK); + delay(1); + + // Enable data ready interrupt + _bus->writeByte(_addr, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY); + delay(1); - // // Set up full scale Accel range. +-16G - _bus->writeByte(_addr, BMI160_RA_ACCEL_RANGE, 0x0C); + // Enable INT1 pin + _bus->writeByte(_addr, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG); delay(1); - // // Set up full scale Gyro range. +-2000dps - _bus->writeByte(_addr, BMI160_RA_GYRO_RANGE, 0x00); + // Map data ready interrupt to INT1 pin + _bus->writeByte(_addr, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY); delay(1); - // Set Accel ODR to 400hz, BWP mode to Oversample 4, LPF of ~40.5hz - _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, 0x0A); + // Set Accel rate 1600HZ + _bus->writeByte(_addr, BMI160_RA_ACCEL_CONF, BMI160_ACCEL_RATE_1600HZ); delay(1); - // Set Gyro ODR to 400hz, BWP mode to Oversample 4, LPF of ~34.15hz - _bus->writeByte(_addr, BMI160_RA_GYRO_CONF, 0x0A); + // Set Gyro rate 3200HZ + _bus->writeByte(_addr, BMI160_RA_GYRO_CONF, BMI160_GYRO_RATE_3200HZ); delay(1); return 1; @@ -136,7 +222,38 @@ class GyroBMI160: public GyroDevice int getRate() const override { - return 8000; + uint8_t valRate = 0; + _bus->readBitsBMI160(_addr, BMI160_RA_GYRO_CONF, BMI160_GYRO_RATE_SEL_BIT, BMI160_GYRO_RATE_SEL_LEN, &valRate); + + switch (valRate) + { + case BMI160_GYRO_RATE_3200HZ: + return 3200; + break; + case BMI160_GYRO_RATE_1600HZ: + return 1600; + break; + case BMI160_GYRO_RATE_800HZ: + return 800; + break; + case BMI160_GYRO_RATE_400HZ: + return 400; + break; + case BMI160_GYRO_RATE_200HZ: + return 200; + break; + case BMI160_GYRO_RATE_100HZ: + return 100; + break; + case BMI160_GYRO_RATE_50HZ: + return 50; + break; + case BMI160_GYRO_RATE_25HZ: + return 25; + break; + default: + return 99; //detect error + } } void setRate(int rate) override From 08c494554720444b078033da7c1469a110df5415 Mon Sep 17 00:00:00 2001 From: "nyoman.gunawan" Date: Fri, 15 Sep 2023 09:13:55 +0800 Subject: [PATCH 3/5] add chip id mpu9250 --- lib/Espfc/src/Device/GyroMPU9250.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Espfc/src/Device/GyroMPU9250.h b/lib/Espfc/src/Device/GyroMPU9250.h index d9112710..21648a74 100644 --- a/lib/Espfc/src/Device/GyroMPU9250.h +++ b/lib/Espfc/src/Device/GyroMPU9250.h @@ -89,7 +89,7 @@ class GyroMPU9250: public GyroMPU6050 uint8_t whoami = 0; _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); //D("mpu9250:whoami", _addr, whoami); - return whoami == 0x71 || whoami == 0x73; + return whoami == 0x71 || whoami == 0x73 || whoami == 0x75; } }; From ad61a144372fbb5c0fb926bc2dac3c177a7ac1b3 Mon Sep 17 00:00:00 2001 From: "nyoman.gunawan" Date: Fri, 15 Sep 2023 17:03:21 +0800 Subject: [PATCH 4/5] optional script esp32 merge bin file --- merge_firmware.py | 43 +++++++++++++++++++++++++++++++++++++++++++ platformio.ini | 1 + 2 files changed, 44 insertions(+) create mode 100644 merge_firmware.py diff --git a/merge_firmware.py b/merge_firmware.py new file mode 100644 index 00000000..b405cbc5 --- /dev/null +++ b/merge_firmware.py @@ -0,0 +1,43 @@ +Import("env") + +APP_BIN = "$BUILD_DIR/${PROGNAME}.bin" +MERGED_BIN = "$BUILD_DIR/${PROGNAME}_esp32_0x00.bin" +BOARD_CONFIG = env.BoardConfig() + + +def merge_bin(source, target, env): + # The list contains all extra images (bootloader, partitions, eboot) and + # the final application binary + flash_images = env.Flatten(env.get("FLASH_EXTRA_IMAGES", [])) + ["$ESP32_APP_OFFSET", APP_BIN] + + # Run esptool to merge images into a single binary + env.Execute( + " ".join( + [ + "$PYTHONEXE", + "$OBJCOPY", + "--chip", + BOARD_CONFIG.get("build.mcu", "esp32"), + "merge_bin", + "--flash_size", + BOARD_CONFIG.get("upload.flash_size", "4MB"), + "-o", + MERGED_BIN, + ] + + flash_images + ) + ) + +# Add a post action that runs esptoolpy to merge available flash images +env.AddPostAction(APP_BIN , merge_bin) + +# Patch the upload command to flash the merged binary at address 0x0 +env.Replace( + UPLOADERFLAGS=[ + f + for f in env.get("UPLOADERFLAGS") + if f not in env.Flatten(env.get("FLASH_EXTRA_IMAGES")) + ] + + ["0x0", MERGED_BIN], + UPLOADCMD='"$PYTHONEXE" "$UPLOADER" $UPLOADERFLAGS', +) \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index c734cbb7..58b0fb89 100644 --- a/platformio.ini +++ b/platformio.ini @@ -55,6 +55,7 @@ upload_speed = ${common.esp32_upload_speed} upload_port = ${common.esp32_upload_port} monitor_speed = ${common.esp32_monitor_speed} monitor_port = ${common.esp32_monitor_port} +;extra_scripts = merge_firmware.py [env:esp8266] board = d1_mini From d8e7d0cce75ea49fb16815a8d8e8666fa1fe09b8 Mon Sep 17 00:00:00 2001 From: "nyoman.gunawan" Date: Fri, 15 Sep 2023 19:31:50 +0800 Subject: [PATCH 5/5] MPU 92.65 id 0x75 same as MPU6500 6-axis --- lib/Espfc/src/Device/GyroMPU6500.h | 3 ++- lib/Espfc/src/Device/GyroMPU9250.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/lib/Espfc/src/Device/GyroMPU6500.h b/lib/Espfc/src/Device/GyroMPU6500.h index f899dc7a..7fb7bf3e 100644 --- a/lib/Espfc/src/Device/GyroMPU6500.h +++ b/lib/Espfc/src/Device/GyroMPU6500.h @@ -17,6 +17,7 @@ #define MPU6500_ACCEL_CONF2 0x1D #define MPU6500_WHOAMI_DEFAULT_VALUE 0x70 +#define MPU6555_WHOAMI_DEFAULT_VALUE 0x75 namespace Espfc { @@ -91,7 +92,7 @@ class GyroMPU6500: public GyroMPU6050 uint8_t whoami = 0; _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); //D("MPU6500:whoami", _addr, whoami); - return whoami == MPU6500_WHOAMI_DEFAULT_VALUE; + return whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6555_WHOAMI_DEFAULT_VALUE; } }; diff --git a/lib/Espfc/src/Device/GyroMPU9250.h b/lib/Espfc/src/Device/GyroMPU9250.h index 21648a74..d9112710 100644 --- a/lib/Espfc/src/Device/GyroMPU9250.h +++ b/lib/Espfc/src/Device/GyroMPU9250.h @@ -89,7 +89,7 @@ class GyroMPU9250: public GyroMPU6050 uint8_t whoami = 0; _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami); //D("mpu9250:whoami", _addr, whoami); - return whoami == 0x71 || whoami == 0x73 || whoami == 0x75; + return whoami == 0x71 || whoami == 0x73; } };