Diff: BMI088.cpp

Revision:

25:fe14dbcef82d

Child:

27:973e495f4711

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMI088.cpp	Mon Jan 06 12:49:38 2020 +0000
@@ -0,0 +1,436 @@
+#include "BMI088.h"
+
+BMI088::BMI088(I2C& i2c)
+{
+    this->i2c = &i2c;
+    devAddrAcc = BMI088_ACC_ADDRESS;
+    devAddrGyro = BMI088_GYRO_ADDRESS;
+}
+
+void BMI088::initialize(void)
+{
+    setAccScaleRange(RANGE_3G);
+    setAccOutputDataRate(ODR_100);
+    setAccPowerMode(ACC_ACTIVE);
+
+    setGyroScaleRange(RANGE_500);
+    setGyroOutputDataRate(ODR_200_BW_23);
+    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 * 9.81 / 32768.0f;
+    else if(range == RANGE_6G) accRange = 6 * 9.81 / 32768.0f;
+    else if(range == RANGE_12G) accRange = 12 * 9.81 / 32768.0f;
+    else if(range == RANGE_24G) accRange = 24 * 9.81 / 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 * 0.01745329 / 32768.0f;
+    else if(range == RANGE_1000) gyroRange = 1000 * 0.01745329f / 32768.0f;
+    else if(range == RANGE_500) gyroRange = 500 * 0.01745329f / 32768.0f;
+    else if(range == RANGE_250) gyroRange = 250 * 0.01745329f / 32768.0f;
+    else if(range == RANGE_125) gyroRange = 125 * 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