From 126cf9ff4a3d9d93983ee8256c01ec642dfeb632 Mon Sep 17 00:00:00 2001 From: Lars Wilko Sentse Date: Tue, 11 Jun 2024 17:30:01 +0200 Subject: [PATCH] should be working state (untested) --- README.md | 6 +- include/sta/drivers/bmi088.hpp | 92 ++++++++++++-- include/sta/drivers/bmi088_defs.hpp | 33 ++++- src/bmi088.cpp | 185 ++++++++++++++++++++++++++-- 4 files changed, 298 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 6cd5160..3e79064 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ -# driver-MC3419 +# driver-BMI088 -Driver for the BMI088 inertial measurement unit and gyroscope +Driver for the BMI088 inertial measurement unit and gyroscope. -This driver is currently incomplete and untested +This driver is untested and is currently missing a readme explanation. \ No newline at end of file diff --git a/include/sta/drivers/bmi088.hpp b/include/sta/drivers/bmi088.hpp index e9645dc..a426b1c 100644 --- a/include/sta/drivers/bmi088.hpp +++ b/include/sta/drivers/bmi088.hpp @@ -3,27 +3,105 @@ #include -#include +#include namespace sta { class BMI088 { public: - BMI088(STM32SPIDevice* gyro_device, STM32SPIDevice* accel_device); + enum GyroMode + { + NORMAL_AWAKE = 0x00, + SUSPEND = 0x80, + DEEP_SUSPEND = 0x20 + }; + enum GyroRange + { + TWOTHOUSAND = 0x00, + ONETHOUSAND = 0x01, + FIVEHUNDRED = 0x02, + TWOFIFTY = 0x03, + ONETWENTYFIVE = 0x04 + }; + enum GyroBandwidth + { + TWOTHOUSAND_FIVETHIRTYTWO = 0x00, + TWOTHOUSAND_TWOTHIRTY = 0x01, + ONETHOUSAND_ONESIXTEEN = 0x02, + FOURHUNDRED_FORTYSEVEN = 0x03, + TWOHUNDRED_TWENTYTHREE = 0x04, + ONEHUNDRED_TWELVE = 0x05, + TWOHUNDRED_SIXTYFOUR = 0x06, + ONEHUNDRED_THIRTYTWO = 0x07 + }; + + enum AccelMode + { + OFF = 0x00, + ON = 0x04 + }; + enum AccelRange + { + THREEG = 0x00, + SIXG = 0x01, + TWELVEG = 0x02, + TWENTYFOURG = 0x03 + }; + enum AccelBandwidth + { + OSR4 = 0x08, + OSR2 = 0x09, + NORMAL_BANDWIDTH = 0x0A + }; + enum AccelODR + { + TWELVE_FIVE = 0x05, + TWENTYFIVE = 0x06, + FIFTY = 0x07, + ONEHUNDRED = 0x08, + TWOHUNDRED = 0x09, + FOURHUNDRED = 0x0A, + EIGHTHUNDRED = 0x0B, + ONESIXHUNDRED = 0x0C + }; - bool init(); private: - enum Part{ + enum Part + { GYROSCOPE, ACCELEROMETER }; - bool busRead(Part part, uint8_t reg, uint8_t * buffer, size_t length); + public: + BMI088(STM32SPIDevice* gyro_device, STM32SPIDevice* accel_device); + + bool init(); + + void setGyroscopeMode(GyroMode mode); + void setGyroscopeRange(GyroRange range); + void setGyroscopeBandwidth(GyroBandwidth bandwidth); + void getRotation(float* x, float* y, float* z); + + void setAccelerometerMode(AccelMode mode); + void setAccelerometerRange(AccelRange range); + void setAccelerometerBandwidth(AccelBandwidth bandwidth, AccelODR odr); + void getAcceleration(float* x, float* y, float* z); - bool busWrite(Part part, uint8_t reg, uint8_t * buffer, size_t length); private: - Device * gyro_device, accel_device; + bool busRead(Part part, uint8_t reg, uint8_t * buffer); + bool busWrite(Part part, uint8_t reg, uint8_t value); + + void getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z); + void getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z); + + void convertRawToActual(uint16_t* i, float* f, float f_range); + + Device* gyro_device; + Device* accel_device; + + float f_gyro_range = 0; + float f_accel_range = 0; }; } // namespace sta diff --git a/include/sta/drivers/bmi088_defs.hpp b/include/sta/drivers/bmi088_defs.hpp index 2e78caf..1d1a6ab 100644 --- a/include/sta/drivers/bmi088_defs.hpp +++ b/include/sta/drivers/bmi088_defs.hpp @@ -1,6 +1,37 @@ #ifndef STA_DRIVERS_BMI088_DEFS_HPP #define STA_DRIVERS_BMI088_DEFS_HPP -//TODO +//GENERAL +#define BMI088_READ_MASK 0x80 + + +//GYROSCOPE +#define BMI088_GYRO_CHIP_ID 0x0F + +#define BMI088_REG_GYRO_CHIP_ID 0x00 +#define BMI088_REG_GYRO_LPM1 0x11 +#define BMI088_REG_GYRO_RANGE 0x0F +#define BMI088_REG_GYRO_BANDWIDTH 0x10 +#define BMI088_REG_GYRO_RATE_Z_MSB 0x07 +#define BMI088_REG_GYRO_RATE_Z_LSB 0x06 +#define BMI088_REG_GYRO_RATE_Y_MSB 0x05 +#define BMI088_REG_GYRO_RATE_Y_LSB 0x04 +#define BMI088_REG_GYRO_RATE_X_MSB 0x03 +#define BMI088_REG_GYRO_RATE_X_LSB 0x02 + + +//ACCELEROMETER +#define BMI088_ACC_CHIP_ID 0x1E + +#define BMI088_REG_ACC_CHIP_ID 0x00 +#define BMI088_REG_ACC_PWR_CTRL 0x7D +#define BMI088_REG_ACC_RANGE 0x41 +#define BMI088_REG_ACC_CONF 0x40 +#define BMI088_REG_ACC_Z_MSB 0x17 +#define BMI088_REG_ACC_Z_LSB 0x16 +#define BMI088_REG_ACC_Y_MSB 0x15 +#define BMI088_REG_ACC_Y_LSB 0x14 +#define BMI088_REG_ACC_X_MSB 0x13 +#define BMI088_REG_ACC_X_LSB 0x12 #endif // STA_DRIVERS_BMI088_DEFS_HPP diff --git a/src/bmi088.cpp b/src/bmi088.cpp index 1a105dc..d17a5c1 100644 --- a/src/bmi088.cpp +++ b/src/bmi088.cpp @@ -1,4 +1,4 @@ -#include +#include #include @@ -7,21 +7,192 @@ namespace sta BMI088::BMI088(STM32SPIDevice* gyro_device, STM32SPIDevice* accel_device) : gyro_device{gyro_device}, accel_device(accel_device) { - STA_ASSERT(device != nullptr); + STA_ASSERT(gyro_device != nullptr); + STA_ASSERT(accel_device != nullptr); } bool BMI088::init() { - return false; + uint8_t id; + busRead(GYROSCOPE, BMI088_REG_GYRO_CHIP_ID, &id); + if(id != BMI088_GYRO_CHIP_ID) return false; + + busRead(ACCELEROMETER, BMI088_REG_ACC_CHIP_ID, &id); + if(id != BMI088_ACC_CHIP_ID) return false; + + return true; } - bool BMI088::busRead(Part part, uint8_t reg, uint8_t * buffer, size_t length) + void BMI088::setGyroscopeMode(GyroMode mode) { - return false; + busWrite(GYROSCOPE, BMI088_REG_GYRO_LPM1, mode); + } + void BMI088::setGyroscopeRange(GyroRange range) + { + busWrite(GYROSCOPE, BMI088_REG_GYRO_RANGE, range); + + switch(range) + { + case TWOTHOUSAND: + f_gyro_range = 2000; + break; + case ONETHOUSAND: + f_gyro_range = 1000; + break; + case FIVEHUNDRED: + f_gyro_range = 500; + break; + case TWOFIFTY: + f_gyro_range = 250; + break; + case ONETWENTYFIVE: + f_gyro_range = 125; + break; + default: + f_gyro_range = 0; + break; + } + } + void BMI088::setGyroscopeBandwidth(GyroBandwidth bandwidth) + { + busWrite(GYROSCOPE, BMI088_REG_GYRO_BANDWIDTH, bandwidth); + } + void BMI088::getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z) + { + uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb; + + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_LSB, &x_lsb); + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_MSB, &x_msb); + *x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb; + + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_LSB, &y_lsb); + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_MSB, &y_msb); + *y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb; + + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_LSB, &z_lsb); + busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_MSB, &z_msb); + *z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb; + + } + void BMI088::getRotation(float* x, float* y, float* z) + { + uint16_t i_x,i_y,i_z; + getRawRotation(&i_x, &i_y, &i_z); + convertRawToActual(&i_x, x, f_gyro_range); + convertRawToActual(&i_y, y, f_gyro_range); + convertRawToActual(&i_z, z, f_gyro_range); } - bool BMI088::busWrite(Part part, uint8_t reg, uint8_t * buffer, size_t length) + + void BMI088::setAccelerometerMode(AccelMode mode) { - return false; + busWrite(ACCELEROMETER, BMI088_REG_ACC_PWR_CTRL, mode); } + void BMI088::setAccelerometerRange(AccelRange range) + { + busWrite(ACCELEROMETER, BMI088_REG_ACC_RANGE, range); + + switch(range) + { + case THREEG: + f_accel_range = 3*9.80665; + break; + case SIXG: + f_accel_range = 6*9.80665; + break; + case TWELVEG: + f_accel_range = 12*9.80665; + break; + case TWENTYFOURG: + f_accel_range = 24*9.80665; + break; + default: + break; + } + } + void BMI088::setAccelerometerBandwidth(AccelBandwidth bandwidth, AccelODR odr) + { + uint8_t data = bandwidth<<4 | odr;//TODO are your sure? shift three or four times? + busWrite(ACCELEROMETER, BMI088_REG_ACC_CONF, data); + } + void BMI088::getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z) + { + uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb; + + busRead(ACCELEROMETER, BMI088_REG_ACC_X_LSB, &x_lsb); + busRead(ACCELEROMETER, BMI088_REG_ACC_X_MSB, &x_msb); + *x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb; + + busRead(ACCELEROMETER, BMI088_REG_ACC_Y_LSB, &y_lsb); + busRead(ACCELEROMETER, BMI088_REG_ACC_Y_MSB, &y_msb); + *y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb; + + busRead(ACCELEROMETER, BMI088_REG_ACC_Z_LSB, &z_lsb); + busRead(ACCELEROMETER, BMI088_REG_ACC_Z_MSB, &z_msb); + *z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb; + + } + void BMI088::getAcceleration(float* x, float* y, float* z) + { + uint16_t i_x,i_y,i_z; + getRawRotation(&i_x, &i_y, &i_z); + convertRawToActual(&i_x, x, f_accel_range); + convertRawToActual(&i_y, y, f_accel_range); + convertRawToActual(&i_z, z, f_accel_range); + } + + + + bool BMI088::busRead(Part part, uint8_t reg, uint8_t * buffer) + { + if(part == GYROSCOPE) + { + uint8_t data[1] = { (uint8_t)( reg | BMI088_READ_MASK ) }; + gyro_device->beginTransmission(); + gyro_device->transfer(data, 1); + gyro_device->receive(buffer, 1); + gyro_device->endTransmission(); + } + if(part == ACCELEROMETER) + { + uint8_t data[2] = { (uint8_t)( reg | BMI088_READ_MASK ), 0x00 }; + accel_device->beginTransmission(); + accel_device->transfer(data, 2); + accel_device->receive(buffer, 1); + accel_device->endTransmission(); + } + return true; + } + + bool BMI088::busWrite(Part part, uint8_t reg, uint8_t value) + { + uint8_t data[2] = { reg, value };//apperently only supports writing 8bit at a time + if(part == GYROSCOPE) + { + gyro_device->beginTransmission(); + gyro_device->transfer(data, 2); + gyro_device->endTransmission(); + } + if(part == ACCELEROMETER) + { + accel_device->beginTransmission(); + accel_device->transfer(data, 2); + accel_device->endTransmission(); + } + return true; + } + + void BMI088::convertRawToActual(uint16_t* i, float* f, float f_range){ + int16_t sign = (int16_t)((*i & 0b1000000000000000) >> 15); + if(sign) + {//negative + uint16_t value = ~(*i) & 0b0111111111111111; + *f = ((float)value / (float)0b0111111111111111) *-f_range; + } + else + {//positive + uint16_t value = *i & 0b0111111111111111; + *f = ((float)value / (float)0b0111111111111111) *f_range; + } + } } // namespace sta