BMI088.cpp

00001 #include "BMI088.h"
00002 
00003 BMI088::BMI088(I2C& i2c)
00004 {
00005     this->i2c = &i2c;
00006     devAddrAcc = BMI088_ACC_ADDRESS;
00007     devAddrGyro = BMI088_GYRO_ADDRESS;
00008     initialize();
00009 }
00010 
00011 void BMI088::initialize(void)
00012 {
00013     setAccScaleRange(RANGE_3G);
00014     wait(.02);
00015     setAccOutputDataRate(ODR_50);  // ODR_50 -> 20 Hz bandwith in normal mode
00016     // setAccOutputDataRateOSR2(ODR_100); // ODR_100 -> 19 Hz bandwith in OSR2 mode
00017     wait(.02);
00018     setAccPowerMode(ACC_ACTIVE);
00019     wait(.02);
00020     setGyroScaleRange(RANGE_500);
00021     wait(.02);
00022     setGyroOutputDataRate(ODR_200_BW_23);
00023     wait(.02);
00024     setGyroPowerMode(GYRO_NORMAL);
00025 }
00026 
00027 bool BMI088::isConnection(void)
00028 {
00029     return ((getAccID() == 0x1E) && (getGyroID() == 0x0F));
00030 }
00031 
00032 void BMI088::resetAcc(void)
00033 {
00034     write8(ACC, BMI088_ACC_SOFT_RESET, 0xB6);
00035 }
00036 
00037 void BMI088::resetGyro(void)
00038 {
00039     write8(GYRO, BMI088_GYRO_SOFT_RESET, 0xB6);
00040 }
00041 
00042 uint8_t BMI088::getAccID(void)
00043 {
00044     return read8(ACC, BMI088_ACC_CHIP_ID);
00045 }
00046 
00047 uint8_t BMI088::getGyroID(void)
00048 {
00049     return read8(GYRO, BMI088_GYRO_CHIP_ID);
00050 }
00051 
00052 void BMI088::setAccPowerMode(acc_power_type_t mode)
00053 {
00054     if(mode == ACC_ACTIVE) {
00055         write8(ACC, BMI088_ACC_PWR_CTRl, 0x04);
00056         write8(ACC, BMI088_ACC_PWR_CONF, 0x00);
00057     } else if(mode == ACC_SUSPEND) {
00058         write8(ACC, BMI088_ACC_PWR_CONF, 0x03);
00059         write8(ACC, BMI088_ACC_PWR_CTRl, 0x00);
00060     }
00061 }
00062 
00063 void BMI088::setGyroPowerMode(gyro_power_type_t mode)
00064 {
00065     if(mode == GYRO_NORMAL) {
00066         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_NORMAL);
00067     } else if(mode == GYRO_SUSPEND) {
00068         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_SUSPEND);
00069     } else if(mode == GYRO_DEEP_SUSPEND) {
00070         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_DEEP_SUSPEND);
00071     }
00072 }
00073 
00074 void BMI088::setAccScaleRange(acc_scale_type_t range)
00075 {
00076     if(range == RANGE_3G) accRange = 3.0f * 9.81f / 32768.0f;
00077     else if(range == RANGE_6G) accRange = 6.0f * 9.81f / 32768.0f;
00078     else if(range == RANGE_12G) accRange = 12.0f * 9.81f / 32768.0f;
00079     else if(range == RANGE_24G) accRange = 24.0f * 9.81f / 32768.0f;
00080 
00081     write8(ACC, BMI088_ACC_RANGE, (uint8_t)range);
00082 }
00083 
00084 void BMI088::setAccOutputDataRate(acc_odr_type_t odr)
00085 {
00086     /*
00087     uint8_t data = 0;
00088 
00089     data = read8(ACC, BMI088_ACC_CONF);
00090     data = data & 0xf0; // 0xf0 := 11110000
00091     data = data | (uint8_t)odr;
00092 
00093     write8(ACC, BMI088_ACC_CONF, data);
00094     */
00095     
00096     uint8_t data = (uint8_t)0xa0 | (uint8_t)odr;
00097     write8(ACC, BMI088_ACC_CONF, data);
00098 }
00099 
00100 void BMI088::setAccOutputDataRateOSR2(acc_odr_type_t odr)
00101 {
00102     uint8_t data = (uint8_t)0x90 | (uint8_t)odr;
00103     write8(ACC, BMI088_ACC_CONF, data);
00104 }
00105 
00106 void BMI088::setAccOutputDataRateOSR4(acc_odr_type_t odr)
00107 {
00108     uint8_t data = (uint8_t)0x80 | (uint8_t)odr;
00109     write8(ACC, BMI088_ACC_CONF, data);
00110 }
00111 
00112 void BMI088::setGyroScaleRange(gyro_scale_type_t range)
00113 {
00114     if(range == RANGE_2000) gyroRange = 2000.0f * 0.01745329f / 32768.0f;
00115     else if(range == RANGE_1000) gyroRange = 1000.0f * 0.01745329f / 32768.0f;
00116     else if(range == RANGE_500) gyroRange = 500.0f * 0.01745329f / 32768.0f;
00117     else if(range == RANGE_250) gyroRange = 250.0f * 0.01745329f / 32768.0f;
00118     else if(range == RANGE_125) gyroRange = 125.0f * 0.01745329f / 32768.0f;
00119 
00120     write8(GYRO, BMI088_GYRO_RANGE, (uint8_t)range);
00121 }
00122 
00123 void BMI088::setGyroOutputDataRate(gyro_odr_type_t odr)
00124 {
00125     write8(GYRO, BMI088_GYRO_BAND_WIDTH, (uint8_t)odr);
00126 }
00127 
00128 void BMI088::getAcceleration(float* x, float* y, float* z)
00129 {
00130     uint8_t buf[6] = {0};
00131     uint16_t ax = 0, ay = 0, az = 0;
00132     float value = 0;
00133 
00134     read(ACC, BMI088_ACC_X_LSB, buf, 6);
00135 
00136     ax = buf[0] | (buf[1] << 8);
00137     ay = buf[2] | (buf[3] << 8);
00138     az = buf[4] | (buf[5] << 8);
00139 
00140     value = (int16_t)ax;
00141     *x = accRange * value ;
00142 
00143     value = (int16_t)ay;
00144     *y = accRange * value;
00145 
00146     value = (int16_t)az;
00147     *z = accRange * value ;
00148 }
00149 
00150 float BMI088::getAccelerationX(void)
00151 {
00152     uint16_t ax = 0;
00153     float value = 0;
00154 
00155     ax = read16(ACC, BMI088_ACC_X_LSB);
00156 
00157     value = (int16_t)ax;
00158     value = accRange * value ;
00159 
00160     return value;
00161 }
00162 
00163 float BMI088::getAccelerationY(void)
00164 {
00165     uint16_t ay = 0;
00166     float value = 0;
00167 
00168     ay = read16(ACC, BMI088_ACC_Y_LSB);
00169 
00170     value = (int16_t)ay;
00171     value = accRange * value ;
00172 
00173     return value;
00174 }
00175 
00176 float BMI088::getAccelerationZ(void)
00177 {
00178     uint16_t az = 0;
00179     float value = 0;
00180 
00181     az = read16(ACC, BMI088_ACC_Z_LSB);
00182 
00183     value = (int16_t)az;
00184     value = accRange * value;
00185 
00186     return value;
00187 }
00188 void BMI088::readAccel(void){
00189     accX = getAccelerationX();
00190     accY = getAccelerationY();
00191     accZ = getAccelerationZ();
00192     }
00193     
00194 
00195 void BMI088::getGyroscope(float* x, float* y, float* z)
00196 {
00197     uint8_t buf[6] = {0};
00198     uint16_t gx = 0, gy = 0, gz = 0;
00199     float value = 0;
00200 
00201     read(GYRO, BMI088_GYRO_RATE_X_LSB, buf, 6);
00202 
00203     gx = buf[0] | (buf[1] << 8);
00204     gy = buf[2] | (buf[3] << 8);
00205     gz = buf[4] | (buf[5] << 8);
00206 
00207     value = (int16_t)gx;
00208     *x = gyroRange * value ;
00209 
00210     value = (int16_t)gy;
00211     *y = gyroRange * value ;
00212 
00213     value = (int16_t)gz;
00214     *z = gyroRange * value ;
00215 }
00216 
00217 float BMI088::getGyroscopeX(void)
00218 {
00219     uint16_t gx = 0;
00220     float value = 0;
00221 
00222     gx = read16(GYRO, BMI088_GYRO_RATE_X_LSB);
00223     value = (int16_t)gx;
00224     value = gyroRange * value;
00225     return value;
00226 }
00227 
00228 float BMI088::getGyroscopeY(void)
00229 {
00230     uint16_t gy = 0;
00231     float value = 0;
00232 
00233     gy = read16(GYRO, BMI088_GYRO_RATE_Y_LSB);
00234 
00235     value = (int16_t)gy;
00236     value = gyroRange * value ;
00237 
00238     return value;
00239 }
00240 
00241 float BMI088::getGyroscopeZ(void)
00242 {
00243     uint16_t gz = 0;
00244     float value = 0;
00245 
00246     gz = read16(GYRO, BMI088_GYRO_RATE_Z_LSB);
00247 
00248     value = (int16_t)gz;
00249     value = gyroRange * value ;
00250 
00251     return value;
00252 }
00253 void BMI088::readGyro(void){
00254     gyroX = getGyroscopeX();
00255     gyroY = getGyroscopeY();
00256     gyroZ = getGyroscopeZ();
00257     }
00258 int16_t BMI088::getTemperature(void)
00259 {
00260     int16_t data = 0;
00261 
00262     data = read16Be(ACC, BMI088_ACC_TEMP_MSB);
00263     data = data >> 5;
00264 
00265     if(data > 1023) data = data - 2048;
00266 
00267     return (int16_t)(data / 8 + 23);
00268 }
00269 
00270 void BMI088::write8(device_type_t dev, uint8_t reg, uint8_t val)
00271 {
00272     uint8_t addr = 0;
00273 
00274     if(dev) addr = devAddrGyro;
00275     else addr = devAddrAcc;
00276 
00277     i2c->start();
00278     if(i2c->write(addr << 1 | 0) != 1) {
00279         return;
00280     }
00281     if(i2c->write(reg) != 1 ) {
00282         return;
00283     }
00284     if(i2c->write(val) != 1 ) {
00285         return;
00286     }
00287     i2c->stop();
00288 
00289 
00290 }
00291 
00292 uint8_t BMI088::read8(device_type_t dev, uint8_t reg)
00293 {
00294     uint8_t addr = 0, data = 0;
00295 
00296     if(dev) addr = devAddrGyro;
00297     else addr = devAddrAcc;
00298 
00299     i2c->start();
00300     if(i2c->write(addr << 1 | 0) != 1) {
00301         return 0xff;
00302     }
00303 
00304     if(i2c->write(reg)!=1) {
00305         return 0xff;
00306     }
00307     //i2c->stop();
00308 
00309 
00310     i2c->start();
00311     if(i2c->write(addr<<1|1)!=1) {
00312         return 0xff;
00313     }
00314 
00315     data = i2c->read(0);
00316     i2c->stop();
00317 
00318     return data;
00319 }
00320 
00321 uint16_t BMI088::read16(device_type_t dev, uint8_t reg)
00322 {
00323     uint8_t addr = 0;
00324     uint16_t msb = 0, lsb = 0;
00325 
00326     if(dev) addr = devAddrGyro;
00327     else addr = devAddrAcc;
00328 
00329     i2c->start();
00330     if(i2c->write(addr << 1 | 0) != 1) {
00331         return 0xffff;
00332     }
00333 
00334     if(i2c->write(reg) != 1) {
00335         return 0xffff;
00336     }
00337     //i2c->stop();
00338 
00339 
00340     i2c->start();
00341     if(i2c->write(addr << 1 | 1) != 1) {
00342         return 0xffff;
00343     }
00344 
00345     //i2c->start();
00346     lsb = i2c->read(1);
00347     msb = i2c->read(0);
00348     i2c->stop();
00349 
00350     return (lsb | (msb << 8));
00351 }
00352 
00353 // bei der Temp sind MSB und LSB vertauscht
00354 uint16_t BMI088::read16Be(device_type_t dev, uint8_t reg)
00355 {
00356     uint8_t addr = 0;
00357     uint16_t msb = 0, lsb = 0;
00358 
00359     if(dev) addr = devAddrGyro;
00360     else addr = devAddrAcc;
00361 
00362     i2c->start();
00363     if(i2c->write(addr << 1 | 0) != 1) {
00364         return 0xffff;
00365     }
00366 
00367     if(i2c->write(reg) != 1) {
00368         return 0xffff;
00369     }
00370     //i2c->stop();
00371 
00372 
00373     i2c->start();
00374     if(i2c->write(addr << 1 | 1) != 1) {
00375         return 0xffff;
00376     }
00377 
00378     msb = i2c->read(1);
00379     lsb = i2c->read(0);
00380     i2c->stop();
00381 
00382     return (lsb | (msb << 8));
00383 }
00384 
00385 uint32_t BMI088::read24(device_type_t dev, uint8_t reg)
00386 {
00387     uint8_t addr = 0;
00388     uint32_t hsb = 0, msb = 0, lsb = 0;
00389 
00390     i2c->start();
00391     if(i2c->write(addr<<1|0)!=1) {
00392         return 0xffffff;
00393     }
00394 
00395     if(i2c->write(reg)!=1) {
00396         return 0xffffff;
00397     }
00398     i2c->stop();
00399 
00400 
00401     i2c->start();
00402     if(i2c->write(addr<<1|1)!=1) {
00403         return 0xffffff;
00404     }
00405 
00406     lsb = i2c->read(1);
00407     msb = i2c->read(1);
00408     hsb = i2c->read(0);
00409     i2c->stop();
00410 
00411     if(dev) addr = devAddrGyro;
00412     else addr = devAddrAcc;
00413 
00414     return (lsb | (msb << 8) | (hsb << 16));
00415 }
00416 
00417 void BMI088::read(device_type_t dev, uint8_t reg, uint8_t *buf, uint16_t len)
00418 {
00419     uint8_t addr = 0;
00420 
00421     if(dev) addr = devAddrGyro;
00422     else addr = devAddrAcc;
00423 
00424     i2c->start();
00425     if(i2c->write(addr<<1|0)!=1) {
00426         for(int in(0); in < len; in++) {
00427             buf[in] = 0xff;
00428         }
00429         return;
00430     }
00431     
00432     if(i2c->write(reg)!=1) {/*
00433         for(int in(0); in < len; in++) {
00434             buf[in] = 0xff;
00435         }
00436         return;*/
00437     }
00438     i2c->stop();
00439 
00440 
00441     i2c->start();
00442     if(i2c->write(addr<<1|1)!=1) {
00443         for(int in(0); in < len; in++) {
00444             buf[in] = 0xff;
00445         }
00446         return;
00447     }
00448 
00449     for(int in = 0; in < len; in++) {
00450         buf[in] = i2c->read(1);
00451     }
00452     i2c->stop();
00453     return;
00454 
00455 }
00456 
00457 //BMI088