AHRS
Dependencies: Eigen
BMI088.cpp
- Committer:
- altb2
- Date:
- 2020-01-20
- Revision:
- 28:21dfb161c67c
- Parent:
- 27:973e495f4711
- Child:
- 29:cd963a6d31c5
File content as of revision 28:21dfb161c67c:
#include "BMI088.h" BMI088::BMI088(I2C& i2c) { this->i2c = &i2c; devAddrAcc = BMI088_ACC_ADDRESS; devAddrGyro = BMI088_GYRO_ADDRESS; initialize(); } void BMI088::initialize(void) { setAccScaleRange(RANGE_3G); wait(.02); setAccOutputDataRate(ODR_100); // ODR_100 -> 40 Hz bandwith in normal mode wait(.02); setAccPowerMode(ACC_ACTIVE); wait(.02); setGyroScaleRange(RANGE_500); wait(.02); setGyroOutputDataRate(ODR_400_BW_47); wait(.02); setGyroPowerMode(GYRO_NORMAL); } bool BMI088::isConnection(void) { return ((getAccID() == 0x1E) && (getGyroID() == 0x0F)); } void BMI088::resetAcc(void) { write8(ACC, BMI088_ACC_SOFT_RESET, 0xB6); } void BMI088::resetGyro(void) { write8(GYRO, BMI088_GYRO_SOFT_RESET, 0xB6); } uint8_t BMI088::getAccID(void) { return read8(ACC, BMI088_ACC_CHIP_ID); } uint8_t BMI088::getGyroID(void) { return read8(GYRO, BMI088_GYRO_CHIP_ID); } void BMI088::setAccPowerMode(acc_power_type_t mode) { if(mode == ACC_ACTIVE) { write8(ACC, BMI088_ACC_PWR_CTRl, 0x04); write8(ACC, BMI088_ACC_PWR_CONF, 0x00); } else if(mode == ACC_SUSPEND) { write8(ACC, BMI088_ACC_PWR_CONF, 0x03); write8(ACC, BMI088_ACC_PWR_CTRl, 0x00); } } void BMI088::setGyroPowerMode(gyro_power_type_t mode) { if(mode == GYRO_NORMAL) { write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_NORMAL); } else if(mode == GYRO_SUSPEND) { write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_SUSPEND); } else if(mode == GYRO_DEEP_SUSPEND) { write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_DEEP_SUSPEND); } } void BMI088::setAccScaleRange(acc_scale_type_t range) { if(range == RANGE_3G) accRange = 3.0f * 9.81f / 32768.0f; else if(range == RANGE_6G) accRange = 6.0f * 9.81f / 32768.0f; else if(range == RANGE_12G) accRange = 12.0f * 9.81f / 32768.0f; else if(range == RANGE_24G) accRange = 24.0f * 9.81f / 32768.0f; write8(ACC, BMI088_ACC_RANGE, (uint8_t)range); } void BMI088::setAccOutputDataRate(acc_odr_type_t odr) { uint8_t data = 0; data = read8(ACC, BMI088_ACC_CONF); data = data & 0xf0; data = data | (uint8_t)odr; write8(ACC, BMI088_ACC_CONF, data); } void BMI088::setGyroScaleRange(gyro_scale_type_t range) { if(range == RANGE_2000) gyroRange = 2000.0f * 0.01745329f / 32768.0f; else if(range == RANGE_1000) gyroRange = 1000.0f * 0.01745329f / 32768.0f; else if(range == RANGE_500) gyroRange = 500.0f * 0.01745329f / 32768.0f; else if(range == RANGE_250) gyroRange = 250.0f * 0.01745329f / 32768.0f; else if(range == RANGE_125) gyroRange = 125.0f * 0.01745329f / 32768.0f; write8(GYRO, BMI088_GYRO_RANGE, (uint8_t)range); } void BMI088::setGyroOutputDataRate(gyro_odr_type_t odr) { write8(GYRO, BMI088_GYRO_BAND_WIDTH, (uint8_t)odr); } void BMI088::getAcceleration(float* x, float* y, float* z) { uint8_t buf[6] = {0}; uint16_t ax = 0, ay = 0, az = 0; float value = 0; read(ACC, BMI088_ACC_X_LSB, buf, 6); ax = buf[0] | (buf[1] << 8); ay = buf[2] | (buf[3] << 8); az = buf[4] | (buf[5] << 8); value = (int16_t)ax; *x = accRange * value ; value = (int16_t)ay; *y = accRange * value; value = (int16_t)az; *z = accRange * value ; } float BMI088::getAccelerationX(void) { uint16_t ax = 0; float value = 0; ax = read16(ACC, BMI088_ACC_X_LSB); value = (int16_t)ax; value = accRange * value ; return value; } float BMI088::getAccelerationY(void) { uint16_t ay = 0; float value = 0; ay = read16(ACC, BMI088_ACC_Y_LSB); value = (int16_t)ay; value = accRange * value ; return value; } float BMI088::getAccelerationZ(void) { uint16_t az = 0; float value = 0; az = read16(ACC, BMI088_ACC_Z_LSB); value = (int16_t)az; value = accRange * value; return value; } void BMI088::readAccel(void){ accX = getAccelerationX(); accY = getAccelerationY(); accZ = getAccelerationZ(); } void BMI088::getGyroscope(float* x, float* y, float* z) { uint8_t buf[6] = {0}; uint16_t gx = 0, gy = 0, gz = 0; float value = 0; read(GYRO, BMI088_GYRO_RATE_X_LSB, buf, 6); gx = buf[0] | (buf[1] << 8); gy = buf[2] | (buf[3] << 8); gz = buf[4] | (buf[5] << 8); value = (int16_t)gx; *x = gyroRange * value ; value = (int16_t)gy; *y = gyroRange * value ; value = (int16_t)gz; *z = gyroRange * value ; } float BMI088::getGyroscopeX(void) { uint16_t gx = 0; float value = 0; gx = read16(GYRO, BMI088_GYRO_RATE_X_LSB); value = (int16_t)gx; value = gyroRange * value; return value; } float BMI088::getGyroscopeY(void) { uint16_t gy = 0; float value = 0; gy = read16(GYRO, BMI088_GYRO_RATE_Y_LSB); value = (int16_t)gy; value = gyroRange * value ; return value; } float BMI088::getGyroscopeZ(void) { uint16_t gz = 0; float value = 0; gz = read16(GYRO, BMI088_GYRO_RATE_Z_LSB); value = (int16_t)gz; value = gyroRange * value ; return value; } void BMI088::readGyro(void){ gyroX = getGyroscopeX(); gyroY = getGyroscopeY(); gyroZ = getGyroscopeZ(); } int16_t BMI088::getTemperature(void) { int16_t data = 0; data = read16Be(ACC, BMI088_ACC_TEMP_MSB); data = data >> 5; if(data > 1023) data = data - 2048; return (int16_t)(data / 8 + 23); } void BMI088::write8(device_type_t dev, uint8_t reg, uint8_t val) { uint8_t addr = 0; if(dev) addr = devAddrGyro; else addr = devAddrAcc; i2c->start(); if(i2c->write(addr << 1 | 0) != 1) { return; } if(i2c->write(reg) != 1 ) { return; } if(i2c->write(val) != 1 ) { return; } i2c->stop(); } uint8_t BMI088::read8(device_type_t dev, uint8_t reg) { uint8_t addr = 0, data = 0; if(dev) addr = devAddrGyro; else addr = devAddrAcc; i2c->start(); if(i2c->write(addr << 1 | 0) != 1) { return 0xff; } if(i2c->write(reg)!=1) { return 0xff; } //i2c->stop(); i2c->start(); if(i2c->write(addr<<1|1)!=1) { return 0xff; } data = i2c->read(0); i2c->stop(); return data; } uint16_t BMI088::read16(device_type_t dev, uint8_t reg) { uint8_t addr = 0; uint16_t msb = 0, lsb = 0; if(dev) addr = devAddrGyro; else addr = devAddrAcc; i2c->start(); if(i2c->write(addr << 1 | 0) != 1) { return 0xffff; } if(i2c->write(reg) != 1) { return 0xffff; } //i2c->stop(); i2c->start(); if(i2c->write(addr << 1 | 1) != 1) { return 0xffff; } //i2c->start(); lsb = i2c->read(1); msb = i2c->read(0); i2c->stop(); return (lsb | (msb << 8)); } // bei der Temp sind MSB und LSB vertauscht uint16_t BMI088::read16Be(device_type_t dev, uint8_t reg) { uint8_t addr = 0; uint16_t msb = 0, lsb = 0; if(dev) addr = devAddrGyro; else addr = devAddrAcc; i2c->start(); if(i2c->write(addr << 1 | 0) != 1) { return 0xffff; } if(i2c->write(reg) != 1) { return 0xffff; } //i2c->stop(); i2c->start(); if(i2c->write(addr << 1 | 1) != 1) { return 0xffff; } msb = i2c->read(1); lsb = i2c->read(0); i2c->stop(); return (lsb | (msb << 8)); } uint32_t BMI088::read24(device_type_t dev, uint8_t reg) { uint8_t addr = 0; uint32_t hsb = 0, msb = 0, lsb = 0; i2c->start(); if(i2c->write(addr<<1|0)!=1) { return 0xffffff; } if(i2c->write(reg)!=1) { return 0xffffff; } i2c->stop(); i2c->start(); if(i2c->write(addr<<1|1)!=1) { return 0xffffff; } lsb = i2c->read(1); msb = i2c->read(1); hsb = i2c->read(0); i2c->stop(); if(dev) addr = devAddrGyro; else addr = devAddrAcc; return (lsb | (msb << 8) | (hsb << 16)); } void BMI088::read(device_type_t dev, uint8_t reg, uint8_t *buf, uint16_t len) { uint8_t addr = 0; if(dev) addr = devAddrGyro; else addr = devAddrAcc; i2c->start(); if(i2c->write(addr<<1|0)!=1) { for(int in(0); in < len; in++) { buf[in] = 0xff; } return; } if(i2c->write(reg)!=1) {/* for(int in(0); in < len; in++) { buf[in] = 0xff; } return;*/ } i2c->stop(); i2c->start(); if(i2c->write(addr<<1|1)!=1) { for(int in(0); in < len; in++) { buf[in] = 0xff; } return; } for(int in = 0; in < len; in++) { buf[in] = i2c->read(1); } i2c->stop(); return; } //BMI088