BMI088.cpp

00001 /*    
00002  * A library for Grove - 6-Axis Accelerometer&Gyroscope（BMI088）
00003  *   
00004  * Copyright (c) 2018 seeed technology co., ltd.  
00005  * Author      : Wayen Weng  
00006  * Create Time : June 2018
00007  * Change Log  : 
00008  *
00009  * The MIT License (MIT)
00010  *
00011  * Permission is hereby granted, free of charge, to any person obtaining a copy
00012  * of this software and associated documentation files (the "Software"), to deal
00013  * in the Software without restriction, including without limitation the rights
00014  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00015  * copies of the Software, and to permit persons to whom the Software is
00016  * furnished to do so, subject to the following conditions:
00017  * 
00018  * The above copyright notice and this permission notice shall be included in
00019  * all copies or substantial portions of the Software.
00020  * 
00021  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00022  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00023  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00024  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00025  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00026  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00027  * THE SOFTWARE.
00028  */
00029 
00030 #include "BMI088.h"
00031 
00032 BMI088::BMI088(void) : i2c(PA_10,PA_9)//L432KC
00033 {
00034     devAddrAcc = BMI088_ACC_ADDRESS;
00035     devAddrGyro = BMI088_GYRO_ADDRESS;
00036     i2c.frequency(100000);
00037 
00038 }
00039 
00040 void BMI088::initialize(void)
00041 {
00042     setAccPoweMode(ACC_ACTIVE);
00043     wait_ms(150);
00044 //    setAccOutputDataRate(ODR_25);
00045     wait_ms(50);
00046     setAccScaleRange(RANGE_3G);
00047     
00048     setGyroScaleRange(RANGE_500);
00049     setGyroOutputDataRate(ODR_200_BW_23);
00050     setGyroPoweMode(GYRO_NORMAL);
00051 }
00052 
00053 bool BMI088::isConnection(void)
00054 {
00055     uint8_t val1 = getGyroID();
00056     uint8_t val2 = getAccID();
00057     return ((val1 == 0x0F) && (val2 == 0x1E));
00058 }
00059 
00060 void BMI088::resetAcc(void)
00061 {
00062     write8(ACC, BMI088_ACC_SOFT_RESET, 0xB6);
00063 }
00064 
00065 void BMI088::resetGyro(void)
00066 {
00067     write8(GYRO, BMI088_GYRO_SOFT_RESET, 0xB6);
00068 }
00069 
00070 uint8_t BMI088::getAccID(void)
00071 {
00072     return read8(ACC, BMI088_ACC_CHIP_ID);
00073 }
00074 
00075 uint8_t BMI088::getGyroID(void)
00076 {
00077     return read8(GYRO, BMI088_GYRO_CHIP_ID);
00078 }
00079 
00080 void BMI088::setAccPoweMode(acc_power_type_t mode)
00081 {
00082     if(mode == ACC_ACTIVE)
00083     {
00084         write8(ACC, BMI088_ACC_PWR_CTRl, 0x04);
00085         write8(ACC, BMI088_ACC_PWR_CONF, 0x00);
00086     }
00087     else if(mode == ACC_SUSPEND)
00088     {
00089         write8(ACC, BMI088_ACC_PWR_CONF, 0x03);
00090         write8(ACC, BMI088_ACC_PWR_CTRl, 0x00);
00091     }
00092 }
00093 
00094 void BMI088::setGyroPoweMode(gyro_power_type_t mode)
00095 {
00096     if(mode == GYRO_NORMAL)
00097     {
00098         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_NORMAL);
00099     }
00100     else if(mode == GYRO_SUSPEND)
00101     {
00102         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_SUSPEND);
00103     }
00104     else if(mode == GYRO_DEEP_SUSPEND)
00105     {
00106         write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_DEEP_SUSPEND);
00107     }
00108 }
00109 
00110 void BMI088::setAccScaleRange(acc_scale_type_t range)
00111 {
00112     if(range == RANGE_3G) accRange = 3000;
00113     else if(range == RANGE_6G) accRange = 6000;
00114     else if(range == RANGE_12G) accRange = 12000;
00115     else if(range == RANGE_24G) accRange = 24000;
00116     
00117     write8(ACC, BMI088_ACC_RANGE, (uint8_t)range);
00118 }
00119 
00120 void BMI088::setAccOutputDataRate(acc_odr_type_t odr)
00121 {
00122     uint8_t data = 0;
00123     
00124     data = read8(ACC, BMI088_ACC_CONF);
00125     data = data & 0xf0;     // get higher 4 bits
00126     data = data | (uint8_t)odr;
00127     wait_ms(10);
00128     write8(ACC, BMI088_ACC_CONF, data);
00129 }
00130 
00131 void BMI088::setGyroScaleRange(gyro_scale_type_t range)
00132 {
00133     if(range == RANGE_2000) gyroRange = 2000;
00134     else if(range == RANGE_1000) gyroRange = 1000;
00135     else if(range == RANGE_500) gyroRange = 500;
00136     else if(range == RANGE_250) gyroRange = 250;
00137     else if(range == RANGE_125) gyroRange = 125;
00138     
00139     write8(GYRO, BMI088_GYRO_RANGE, (uint8_t)range);
00140 }
00141 
00142 void BMI088::setGyroOutputDataRate(gyro_odr_type_t odr)
00143 {
00144     write8(GYRO, BMI088_GYRO_BAND_WIDTH, (uint8_t)odr);
00145 }
00146 
00147 void BMI088::getAcceleration(float* x, float* y, float* z)
00148 {
00149     char buf[6] = {0};
00150     uint16_t ax = 0, ay = 0, az = 0;
00151     int16_t value = 0;
00152     
00153     read(ACC, BMI088_ACC_X_LSB, buf, 6);
00154 
00155     //printf("ACCBUFF: %02X %02X %02X %02X %02X %02X\r\n",buf[0],buf[1],buf[2],buf[3],buf[4],buf[5]);
00156     ax = buf[0] | (buf[1] << 8);
00157     ay = buf[2] | (buf[3] << 8);
00158     az = buf[4] | (buf[5] << 8);
00159     
00160     value = (int16_t)ax;
00161     *x = accRange * value / 32768000 * 9.81f;        // units: m/s^2
00162     
00163     value = (int16_t)ay;
00164     *y = accRange * value / 32768000 * 9.81f;        // units: m/s^2
00165     
00166     value = (int16_t)az;
00167     *z = accRange * value / 32768000 * 9.81f;        // units: m/s^2
00168 }
00169 
00170 float BMI088::getAccelerationX(void)
00171 {
00172     uint16_t ax = 0;
00173     float value = 0;
00174     
00175     ax = read16(ACC, BMI088_ACC_X_LSB);
00176     
00177     value = (int16_t)ax;
00178     value = accRange * value / 32768000 * 9.81f;     // units: m/s^2
00179     
00180     return value;
00181 }
00182 
00183 float BMI088::getAccelerationY(void)
00184 {
00185     uint16_t ay = 0;
00186     float value = 0;
00187     
00188     ay = read16(ACC, BMI088_ACC_Y_LSB);
00189     
00190     value = (int16_t)ay;
00191     value = accRange * value / 32768000 * 9.81f;     // units: m/s^2
00192     
00193     return value;
00194 }
00195 
00196 float BMI088::getAccelerationZ(void)
00197 {
00198     uint16_t az = 0;
00199     float value = 0;
00200     
00201     az = read16(ACC, BMI088_ACC_Z_LSB);
00202     
00203     
00204     value = (int16_t)az;
00205     value = accRange * value / 32768000 * 9.81f;     // units: m/s^2
00206     
00207     return value;
00208 }
00209 
00210 void BMI088::getGyroscope(float* x, float* y, float* z)
00211 {
00212     char buf[6] = {0};
00213     uint16_t gx = 0, gy = 0, gz = 0;
00214     int16_t value = 0;
00215     
00216     read(GYRO, BMI088_GYRO_RATE_X_LSB, buf, 6);
00217     
00218     gx = buf[0] | (buf[1] << 8);
00219     gy = buf[2] | (buf[3] << 8);
00220     gz = buf[4] | (buf[5] << 8);
00221     
00222     value = (int16_t)gx;
00223     *x = gyroRange * value / 32768 * 3.1415927f/180.0f;
00224     
00225     value = (int16_t)gy;
00226     *y = gyroRange * value / 32768 * 3.1415927f/180.0f;
00227     
00228     value = (int16_t)gz;
00229     *z = gyroRange * value / 32768 * 3.1415927f/180.0f;
00230 }
00231 
00232 float BMI088::getGyroscopeX(void)
00233 {
00234     uint16_t gx = 0;
00235     float value = 0;
00236     
00237     gx = read16(GYRO, BMI088_GYRO_RATE_X_LSB);
00238     
00239     value = (int16_t)gx;
00240     value = gyroRange * value / 32768 * 3.1415927f/180.0f;
00241     
00242     return value;
00243 }
00244 
00245 float BMI088::getGyroscopeY(void)
00246 {
00247     uint16_t gy = 0;
00248     float value = 0;
00249     
00250     gy = read16(GYRO, BMI088_GYRO_RATE_Y_LSB);
00251     
00252     value = (int16_t)gy;
00253     value = gyroRange * value / 32768 * 3.1415927f/180.0f;
00254     
00255     return value;
00256 }
00257 
00258 float BMI088::getGyroscopeZ(void)
00259 {
00260     uint16_t gz = 0;
00261     float value = 0;
00262     
00263     gz = read16(GYRO, BMI088_GYRO_RATE_Z_LSB);
00264     
00265     value = (int16_t)gz;
00266     value = gyroRange * value / 32768 * 3.1415927f/180.0f;
00267     
00268     return value;
00269 }
00270 
00271 uint16_t BMI088::getTemperature(void)
00272 {
00273     uint16_t data = 0;
00274     
00275     data = read16Be(ACC, BMI088_ACC_TEMP_MSB);
00276     data = data >> 5;
00277     
00278     if(data > 1023) data = data - 2048;
00279     
00280     return (uint16_t)(data / 8 + 23);
00281 }
00282 
00283 void BMI088::write8(device_type_t dev, char reg, uint8_t val)
00284 {
00285     int addr = 0;
00286     
00287     if(dev) addr = devAddrGyro;
00288     else addr = devAddrAcc;
00289     
00290     int succw = i2c.write(addr, &reg, val); 
00291  //   printf("Write 8 succes: %d\r\n",succw);
00292     
00293     /*Wire.beginTransmission(addr);
00294     Wire.write(reg);
00295     Wire.write(val);
00296     Wire.endTransmission();*/
00297 }
00298 
00299 char BMI088::read8(device_type_t dev, char reg)
00300 {
00301     int addr =0;;
00302     char data = 0;
00303     
00304     if(dev) addr = devAddrGyro;
00305     else addr = devAddrAcc;
00306     
00307     int succw = i2c.write(addr, &reg, 0x01); 
00308     int succr = i2c.read( addr, &data, 0x01);
00309     
00310     /*Wire.beginTransmission(addr);
00311     Wire.write(reg);
00312     Wire.endTransmission();
00313 
00314     Wire.requestFrom(addr, 1);
00315     while(Wire.available())
00316     {
00317         data = Wire.read();
00318     }
00319     */
00320     return data;
00321 }
00322 
00323 uint16_t BMI088::read16(device_type_t dev, char reg)
00324 {
00325     int addr = 0;
00326     char buf[2];
00327     
00328     if(dev) addr = devAddrGyro;
00329     else addr = devAddrAcc;
00330 
00331     int succw = i2c.write(addr, &reg, 0x01); 
00332     int succr = i2c.read( addr, buf, 0x02);
00333 
00334 
00335     /*Wire.beginTransmission(addr);
00336     Wire.write(reg);
00337     Wire.endTransmission();
00338     
00339     Wire.requestFrom(addr, 2);
00340     while(Wire.available())
00341     {
00342         lsb = Wire.read();
00343         msb = Wire.read();
00344     }
00345 */
00346     return (buf[0] | (buf[1] << 8));
00347 }
00348 
00349 uint16_t BMI088::read16Be(device_type_t dev, char reg)
00350 {
00351     uint8_t addr = 0;
00352     uint16_t msb = 0, lsb = 0;
00353     
00354     if(dev) addr = devAddrGyro;
00355     else addr = devAddrAcc;
00356 
00357   /*  Wire.beginTransmission(addr);
00358     Wire.write(reg);
00359     Wire.endTransmission();
00360     
00361     Wire.requestFrom(addr, 2);
00362     while(Wire.available())
00363     {
00364         msb = Wire.read();
00365         lsb = Wire.read();
00366     }
00367 */
00368     return (lsb | (msb << 8));
00369 }
00370 
00371 uint32_t BMI088::read24(device_type_t dev, char reg)
00372 {
00373     int addr;
00374     char data = 0;
00375     
00376     if(dev) addr = devAddrGyro;
00377     else addr = devAddrAcc;
00378 
00379 /*    Wire.beginTransmission(addr);
00380     Wire.write(reg);
00381     Wire.endTransmission();
00382     
00383     Wire.requestFrom(addr, 3);
00384     while(Wire.available())
00385     {
00386         lsb = Wire.read();
00387         msb = Wire.read();
00388         hsb = Wire.read();
00389     }
00390 */
00391     return 0;//(lsb | (msb << 8) | (hsb << 16));
00392 }
00393 
00394 void BMI088::read(device_type_t dev, char reg, char *buf, uint16_t len)
00395 {
00396     int addr;
00397         
00398     if(dev) addr = devAddrGyro;
00399     else addr = devAddrAcc;
00400     int succw = i2c.write(addr, &reg, 0x01); 
00401     int succr = i2c.read( addr, buf, len);
00402     //printf("\r\nread: succes w: %d, succes r: %d, buf[1]:%d\r\n",succw,succr,buf[1]);
00403   /*  
00404     Wire.beginTransmission(addr);
00405     Wire.write(reg);
00406     Wire.endTransmission();
00407     
00408     Wire.requestFrom(addr, len);
00409     while(Wire.available())
00410     {
00411         for(uint16_t i = 0; i < len; i ++) buf[i] = Wire.read();
00412     }
00413     */
00414 }