#include #include #include #define GRAVITY 9.80665f #include #ifndef STA_SPATZ_ENABLED namespace sta { BMI088::BMI088(SPIDevice* gyro_device, SPIDevice* accel_device) : gyro_device{gyro_device}, accel_device(accel_device) { STA_ASSERT(gyro_device != nullptr); STA_ASSERT(accel_device != nullptr); } bool BMI088::init() { uint8_t id; busRead(GYROSCOPE, BMI088_REG_GYRO_CHIP_ID, &id); if(id != BMI088_GYRO_CHIP_ID) return false; setGyroscopeRange(GyroRange::_1000); setGyroscopeBandwidth(GyroBandwidth::_200_64); setGyroscopeMode(GyroMode::NORMAL_AWAKE); busRead(ACCELEROMETER, BMI088_REG_ACC_CHIP_ID, &id); if(id != BMI088_ACC_CHIP_ID) return false; setAccelerometerRange(AccelRange::_24G); setAccelerometerBandwidth(AccelBandwidth::NORMAL_BANDWIDTH, AccelODR::_100); setAccelerometerMode(AccelMode::ON); return true; } void BMI088::setGyroscopeMode(GyroMode mode) { busWrite(GYROSCOPE, BMI088_REG_GYRO_LPM1, mode); } void BMI088::setGyroscopeRange(GyroRange range) { busWrite(GYROSCOPE, BMI088_REG_GYRO_RANGE, range); switch(range) { case _2000: f_gyro_range = 2000; break; case _1000: f_gyro_range = 1000; break; case _500: f_gyro_range = 500; break; case _250: f_gyro_range = 250; break; case _125: 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(int16_t* x, int16_t* y, int16_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 = (int16_t)(((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 = (int16_t)(((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 = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb); } void BMI088::getRotation(float* x, float* y, float* z) { int16_t i_x,i_y,i_z; getRawRotation(&i_x, &i_y, &i_z); *x = lsbToDPS(i_x); *y = lsbToDPS(i_y); *z = lsbToDPS(i_z); } void BMI088::setAccelerometerMode(AccelMode mode) { busWrite(ACCELEROMETER, BMI088_REG_ACC_PWR_CTRL, mode); } void BMI088::setAccelerometerRange(AccelRange range) { busWrite(ACCELEROMETER, BMI088_REG_ACC_RANGE, range); switch(range) { case _3G: f_accel_range = 3*GRAVITY; break; case _6G: f_accel_range = 6*GRAVITY; break; case _12G: f_accel_range = 12*GRAVITY; break; case _24G: f_accel_range = 24*GRAVITY; 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(int16_t* x, int16_t* y, int16_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 = (int16_t)(((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 = (int16_t)(((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 = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb); } void BMI088::getAcceleration(float* x, float* y, float* z) { int16_t i_x,i_y,i_z; getRawAcceleration(&i_x, &i_y, &i_z); *x = lsbToMps2(i_x); *y = lsbToMps2(i_y); *z = lsbToMps2(i_z); } 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; } float BMI088::lsbToMps2(int16_t val) { double power = 2; float half_scale = (float)((pow((double)power, (double)16) / 2.0f)); return (val * f_accel_range) / half_scale; } float BMI088::lsbToDPS(int16_t val) { double power = 2; float half_scale = (float)((pow((double)power, (double)16) / 2.0f)); return (f_gyro_range / (half_scale)) * (val); } 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 #endif // STA_SPATZ_ENABLED