BMX055.cpp

00001 /*
00002  * mbed library program
00003  *  BMX055 Small, versatile 9-axis sensor module
00004  *  by Bosch Sensortec
00005  *
00006  * Copyright (c) 2018,'19 Kenji Arai / JH1PJL
00007  *  http://www.page.sannet.ne.jp/kenjia/index.html
00008  *  http://mbed.org/users/kenjiArai/
00009  *      Started:    October   24th, 2018
00010  *      Revised:    March      3rd, 2019
00011  */
00012 
00013 #include "mbed.h"
00014 #include "BMX055.h"
00015 
00016 #define DEBUG 0
00017 
00018 #if MBED_MAJOR_VERSION == 2
00019 #define WAIT_MS(x)       wait_ms(x)
00020 #elif  MBED_MAJOR_VERSION == 5
00021 #define WAIT_MS(x)       Thread::wait(x)
00022 #else
00023 #error "Running on Unknown OS"
00024 #endif
00025 
00026 BMX055::BMX055 (PinName p_sda, PinName p_scl):
00027     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
00028 {
00029     bmx055_parameters = bmx055_std_paramtr;
00030     initialize ();
00031 }
00032 
00033 BMX055::BMX055 (I2C& p_i2c) :
00034     _i2c(p_i2c)
00035 {
00036     initialize ();
00037 }
00038 
00039 /////////////// Set parameter /////////////////////////////
00040 void BMX055::set_parameter(const BMX055_TypeDef *bmx055_parameter)
00041 {
00042     bmx055_parameters = *bmx055_parameter;
00043     set_parameters_to_regs();
00044     
00045 }
00046 
00047 /////////////// Read data & normalize /////////////////////
00048 void BMX055::get_accel(BMX055_ACCEL_TypeDef *acc)
00049 {
00050     int16_t x,y,z;
00051     float factor = 2.0f;
00052 
00053     chip_addr = inf_addr.acc_addr;
00054     dt[0] = 0x02;
00055     _i2c.write(chip_addr, dt, 1, true);
00056     _i2c.read(chip_addr, dt, 6, false);
00057 #if DEBUG
00058     printf("Read ACC data-> ");
00059     for (uint32_t i = 0; i < 6; i++){
00060         printf("i=%d,dt=0x%02x, ", i, dt[i]);
00061     }
00062     printf(", all\r\n");
00063 #endif
00064     x = dt[1] << 8 | (dt[0] & 0xf0);
00065     y = dt[3] << 8 | (dt[2] & 0xf0);
00066     z = dt[5] << 8 | (dt[4] & 0xf0);
00067     switch(bmx055_parameters.acc_fs){
00068         case ACC_2G:
00069             factor = 2.0f;
00070             break;
00071         case ACC_4G:
00072             factor = 4.0f;
00073             break;
00074         case ACC_8G:
00075             factor = 8.0f;
00076             break;
00077         case ACC_16G:
00078             factor = 16.0f;
00079             break;
00080         default:
00081             factor = 0;
00082             break;
00083     }
00084     acc->x = (double)x * factor / 2048.0f / 16.0f;
00085     acc->y = (double)y * factor / 2048.0f / 16.0f;
00086     acc->z = (double)z * factor / 2048.0f / 16.0f;
00087 }
00088 
00089 void BMX055::get_gyro(BMX055_GYRO_TypeDef *gyr)
00090 {
00091     int16_t x,y,z;
00092     float factor = 2.0f;
00093 
00094     chip_addr = inf_addr.gyr_addr;
00095     dt[0] = 0x02;
00096     _i2c.write(chip_addr, dt, 1, true);
00097     _i2c.read(chip_addr, dt, 6, false);
00098 #if DEBUG
00099     printf("Read MAG data-> ");
00100     for (uint32_t i = 0; i < 6; i++){
00101         printf("i=%d,dt=0x%02x, ", i, dt[i]);
00102     }
00103     printf(", all\r\n");
00104 #endif
00105     x = dt[1] << 8 | dt[0];
00106     y = dt[3] << 8 | dt[2];
00107     z = dt[5] << 8 | dt[4];
00108 #if 0
00109     switch(bmx055_parameters.gyr_fs){
00110         case GYR_2000DPS:
00111             factor = 1998.0f;
00112             break;
00113         case GYR_1000DPS:
00114             factor = 999.0f;
00115             break;
00116         case GYR_500DPS:
00117             factor = 499.5f;
00118             break;
00119         case GYR_250DPS:
00120             factor = 249.75f;
00121             break;
00122         case GYR_125DPS:
00123             factor = 124.87f;
00124             break;
00125         default:
00126             factor = 0;
00127             break;
00128     }
00129     gyr->x = (double)x * factor / 32768.0f / 16.0f;
00130     gyr->y = (double)y * factor / 32768.0f / 16.0f;
00131     gyr->z = (double)z * factor / 32768.0f / 16.0f;
00132 #else
00133     switch(bmx055_parameters.gyr_fs){
00134         case GYR_2000DPS:
00135             factor = 61.0f;
00136             break;
00137         case GYR_1000DPS:
00138             factor = 30.5f;
00139             break;
00140         case GYR_500DPS:
00141             factor = 15.3;
00142             break;
00143         case GYR_250DPS:
00144             factor = 7.6f;
00145             break;
00146         case GYR_125DPS:
00147             factor = 3.8f;
00148             break;
00149         default:
00150             factor = 0;
00151             break;
00152     }
00153     gyr->x = (double)x * factor / 1000.0f;
00154     gyr->y = (double)y * factor / 1000.0f;
00155     gyr->z = (double)z * factor / 1000.0f;
00156 #endif
00157 }
00158 
00159 void BMX055::get_magnet(BMX055_MAGNET_TypeDef *mag)
00160 {
00161     int16_t x,y,z;
00162 
00163     chip_addr = inf_addr.gyr_addr;
00164     dt[0] = 0x42;
00165     _i2c.write(chip_addr, dt, 1, true);
00166     _i2c.read(chip_addr, dt, 6, false);
00167 #if DEBUG
00168     printf("Read GYR data-> ");
00169     for (uint32_t i = 0; i < 6; i++){
00170         printf("i=%d,dt=0x%02x, ", i, dt[i]);
00171     }
00172     printf(", all\r\n");
00173 #endif
00174     x = (dt[1] << 5 | (dt[0] & 0x1f)) << 3;    // 13bit
00175     y = (dt[3] << 5 | (dt[2] & 0x1f)) << 3;    // 13bit
00176     z = (dt[5] << 7 | (dt[4] & 0x7f)) << 1;    // 15bit
00177     mag->x = (double)x;
00178     mag->y = (double)y;
00179     mag->z = (double)z;
00180 }
00181 
00182 float BMX055::get_chip_temperature()
00183 {
00184     chip_addr = inf_addr.acc_addr;
00185     dt[0] = 0x08;   // chip tempareture reg addr
00186     _i2c.write(chip_addr, dt, 1, true);
00187     _i2c.read(chip_addr, dt, 1, false);
00188     //printf("Temp reg = 0x%02x\r\n", dt[0]);
00189     return (float)((int8_t)dt[0]) * 0.5f + 23.0f;
00190 }
00191 
00192 /////////////// Initialize ////////////////////////////////
00193 void BMX055::initialize (void)
00194 {
00195     _i2c.frequency(100000);
00196     // Check Acc & Mag & Gyro are available of not
00197     check_id();
00198     if (ready_flag == 0x07){
00199 #if DEBUG
00200         printf("ACC+GYR+MAG are ready!\r\n");
00201 #endif
00202     }
00203     // Set initial data
00204     set_parameters_to_regs();
00205 }
00206 
00207 ////// Set initialize data to related registers ///////////
00208 void BMX055::set_parameters_to_regs(void)
00209 {
00210     // ACC
00211     chip_addr = inf_addr.acc_addr;
00212     dt[0] = 0x0f;   // Select PMU_Range register
00213     dt[1] = bmx055_parameters.acc_fs;
00214     _i2c.write(chip_addr, dt, 2, false);
00215     wait_ms(1);
00216     dt[0] = 0x10;   // Select PMU_BW register
00217     dt[1] = bmx055_parameters.acc_bw;
00218     _i2c.write(chip_addr, dt, 2, false);
00219     wait_ms(1);
00220     dt[0] = 0x11;   // Select PMU_LPW register
00221     dt[1] = 0x00;   // Normal mode, Sleep duration = 0.5ms
00222     _i2c.write(chip_addr, dt, 2, false);
00223     // GYR
00224     chip_addr = inf_addr.gyr_addr;
00225     dt[0] = 0x0f;   // Select Range register
00226     dt[1] = bmx055_parameters.gyr_fs;
00227     _i2c.write(chip_addr, dt, 2, false);
00228     wait_ms(1);
00229     dt[0] = 0x10;   // Select Bandwidth register
00230     dt[1] = bmx055_parameters.gyr_bw;
00231     _i2c.write(chip_addr, dt, 2, false);
00232     wait_ms(1);
00233     dt[0] = 0x11;   // Select LPM1 register
00234     dt[1] = 0x00;   // Normal mode, Sleep duration = 2ms
00235     _i2c.write(chip_addr, dt, 2, false);
00236     // MAG
00237     chip_addr = inf_addr.mag_addr;
00238     dt[0] = 0x4b;   // Select Mag register
00239     dt[1] = 0x83;   // Soft reset
00240     _i2c.write(chip_addr, dt, 2, false);
00241     wait_ms(10);
00242     dt[0] = 0x4b;   // Select Mag register
00243     dt[1] = 0x01;   // Soft reset
00244     _i2c.write(chip_addr, dt, 2, false);
00245     wait_ms(10);
00246     dt[0] = 0x4c;   // Select Mag register
00247     dt[1] = bmx055_parameters.mag_odr;
00248     _i2c.write(chip_addr, dt, 2, false);
00249     wait_ms(1);
00250     dt[0] = 0x4e;   // Select Mag register
00251     dt[1] = 0x84;   // X, Y, Z-Axis enabled
00252     _i2c.write(chip_addr, dt, 2, false);
00253     wait_ms(1);
00254     dt[0] = 0x51;   // Select Mag register
00255     dt[1] = 0x04;   // No. of Repetitions for X-Y Axis = 9
00256     _i2c.write(chip_addr, dt, 2, false);
00257     wait_ms(1);
00258     dt[0] = 0x52;   // Select Mag register
00259     dt[1] = 0x16;   // No. of Repetitions for Z-Axis = 15
00260     _i2c.write(chip_addr, dt, 2, false);
00261 #if 0
00262     // ACC
00263     chip_addr = inf_addr.acc_addr;
00264     dt[0] = 0x0f;   // Select PMU_Range register
00265     dt[1] = 0x03;   // Range = +/- 2g
00266     _i2c.write(chip_addr, dt, 2, false);
00267     wait_ms(1);
00268     dt[0] = 0x10;   // Select PMU_BW register
00269     dt[1] = 0x08;   // Bandwidth = 7.81 Hz
00270     _i2c.write(chip_addr, dt, 2, false);
00271     wait_ms(1);
00272     dt[0] = 0x11;   // Select PMU_LPW register
00273     dt[1] = 0x00;   // Normal mode, Sleep duration = 0.5ms
00274     _i2c.write(chip_addr, dt, 2, false);
00275     wait_ms(1);
00276     // GYR
00277     chip_addr = inf_addr.gyr_addr;
00278     dt[0] = 0x0f;   // Select Range register
00279     dt[1] = 0x04;   // Full scale = +/- 125 degree/s
00280     _i2c.write(chip_addr, dt, 2, false);
00281     wait_ms(1);
00282     dt[0] = 0x10;   // Select Bandwidth register
00283     dt[1] = 0x07;   // ODR = 100 Hz
00284     _i2c.write(chip_addr, dt, 2, false);
00285     wait_ms(1);
00286     dt[0] = 0x11;   // Select LPM1 register
00287     dt[1] = 0x00;   // Normal mode, Sleep duration = 2ms
00288     _i2c.write(chip_addr, dt, 2, false);
00289     // MAG
00290     chip_addr = inf_addr.mag_addr;
00291     dt[0] = 0x4b;   // Select Mag register
00292     dt[1] = 0x83;   // Soft reset
00293     _i2c.write(chip_addr, dt, 2, false);
00294     wait_ms(10);
00295     dt[0] = 0x4b;   // Select Mag register
00296     dt[1] = 0x01;   // Soft reset
00297     _i2c.write(chip_addr, dt, 2, false);
00298     wait_ms(10);
00299     dt[0] = 0x4c;   // Select Mag register
00300     dt[1] = 0x00;   // Normal Mode, ODR = 10 Hz
00301     _i2c.write(chip_addr, dt, 2, false);
00302     wait_ms(1);
00303     dt[0] = 0x4e;   // Select Mag register
00304     dt[1] = 0x84;   // X, Y, Z-Axis enabled
00305     _i2c.write(chip_addr, dt, 2, false);
00306     wait_ms(1);
00307     dt[0] = 0x51;   // Select Mag register
00308     dt[1] = 0x04;   // No. of Repetitions for X-Y Axis = 9
00309     _i2c.write(chip_addr, dt, 2, false);
00310     wait_ms(1);
00311     dt[0] = 0x52;   // Select Mag register
00312     dt[1] = 0x16;   // No. of Repetitions for Z-Axis = 15
00313     _i2c.write(chip_addr, dt, 2, false);
00314     wait_ms(1);
00315 #endif
00316 }
00317 
00318 /////////////// Check Who am I? ///////////////////////////
00319 void BMX055::check_id(void)
00320 {
00321     ready_flag = 0;
00322     // ID ACC
00323     inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR;
00324     chip_addr = inf_addr.acc_addr;
00325     dt[0] = 0x00;   // chip ID reg addr
00326     _i2c.write(chip_addr, dt, 1, true);
00327     _i2c.read(chip_addr, dt, 1, false);
00328     inf_id.acc_id = dt[0];
00329     if (inf_id.acc_id == I_AM_BMX055_ACC) {
00330         ready_flag |= 0x01;
00331     } else {
00332         inf_addr.acc_addr = (0x18 << 1);
00333         chip_addr = inf_addr.acc_addr;
00334         dt[0] = 0x00;   // chip ID reg addr
00335         _i2c.write(chip_addr, dt, 1, true);
00336         _i2c.read(chip_addr, dt, 1, false);
00337         inf_id.acc_id = dt[0];
00338         if (inf_id.acc_id == I_AM_BMX055_ACC) {
00339             ready_flag |= 0x01;
00340         }
00341     }
00342     // ID GYRO
00343     inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR;
00344     chip_addr = inf_addr.gyr_addr;
00345     dt[0] = 0x00;   // chip ID reg addr
00346     _i2c.write(chip_addr, dt, 1, true);
00347     _i2c.read(chip_addr, dt, 1, false);
00348     inf_id.gyr_id = dt[0];
00349     if (inf_id.gyr_id == I_AM_BMX055_GYR) {
00350         ready_flag |= 0x02;
00351     } else {
00352         inf_addr.gyr_addr = (0x68 << 1);
00353         chip_addr = inf_addr.gyr_addr;
00354         dt[0] = 0x00;   // chip ID reg addr
00355         _i2c.write(chip_addr, dt, 1, true);
00356         _i2c.read(chip_addr, dt, 1, false);
00357         inf_id.gyr_id = dt[0];
00358         if (inf_id.gyr_id == I_AM_BMX055_GYR) {
00359             ready_flag |= 0x02;
00360         }
00361     }
00362     // ID Mag
00363     inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR;
00364     chip_addr = inf_addr.mag_addr;
00365     dt[0] = 0x4b;   // reg addr
00366     dt[1] = 0x01;   // control power bit set 1
00367     _i2c.write(chip_addr, dt, 2, false);
00368     dt[0] = 0x40;   // chip ID reg addr
00369     _i2c.write(chip_addr, dt, 1, true);
00370     _i2c.read(chip_addr, dt, 1, false);
00371     inf_id.mag_id = dt[0];
00372     if (inf_id.mag_id == I_AM_BMX055_MAG) {
00373         ready_flag |= 0x04;
00374     } else {
00375         inf_addr.mag_addr = (0x12 << 1);
00376         chip_addr = inf_addr.mag_addr;
00377         // control power bit set 1
00378         dt[0] = 0x4b;
00379         dt[1] = 0x01;
00380         _i2c.write(chip_addr, dt, 2, false);
00381         dt[0] = 0x40;
00382         _i2c.write(chip_addr, dt, 1, true);
00383         _i2c.read(chip_addr, dt, 1, false);
00384         inf_id.mag_id = dt[0];
00385         if (inf_id.mag_id == I_AM_BMX055_MAG) {
00386             ready_flag |= 0x04;
00387         } else {
00388             inf_addr.mag_addr = (0x11 << 1);
00389             chip_addr = inf_addr.mag_addr;
00390             // control power bit set 1
00391             dt[0] = 0x4b;
00392             dt[1] = 0x01;
00393             _i2c.write(chip_addr, dt, 2, false);
00394             dt[0] = 0x40;
00395             _i2c.write(chip_addr, dt, 1, true);
00396             _i2c.read(chip_addr, dt, 1, false);
00397             inf_id.mag_id = dt[0];
00398             if (inf_id.mag_id == I_AM_BMX055_MAG) {
00399                 ready_flag |= 0x04;
00400             } else {
00401                 inf_addr.mag_addr = (0x10 << 1);
00402                 chip_addr = inf_addr.mag_addr;
00403                 // control power bit set 1
00404                 dt[0] = 0x4b;
00405                 dt[1] = 0x01;
00406                 _i2c.write(chip_addr, dt, 2, false);
00407                 dt[0] = 0x40;
00408                 _i2c.write(chip_addr, dt, 1, true);
00409                 _i2c.read(chip_addr, dt, 1, false);
00410                 inf_id.mag_id = dt[0];
00411                 if (inf_id.mag_id == I_AM_BMX055_MAG) {
00412                     ready_flag |= 0x04;
00413                 }
00414             }
00415         }
00416     }
00417 #if DEBUG
00418     printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id); 
00419     printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id); 
00420     printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id); 
00421     printf("ready_flag = 0x%x\r\n", ready_flag);
00422 #endif
00423 }
00424 
00425 void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id)
00426 {
00427     id->acc_id = acc_id;
00428     id->mag_id = mag_id;
00429     id->gyr_id = gyr_id;
00430 }
00431 
00432 /////////////// Check chip ready or not  //////////////////
00433 bool BMX055::chip_ready(void)
00434 {
00435     if (ready_flag == 0x07) {
00436         return true;
00437     }
00438     return false;
00439 }
00440 
00441 /////////////// I2C Freq. /////////////////////////////////
00442 void BMX055::frequency(int hz)
00443 {
00444     _i2c.frequency(hz);
00445 }
00446 
00447 /////////////// Read/Write specific register //////////////
00448 uint8_t BMX055::read_reg(uint8_t addr)
00449 {
00450     dt[0] = addr;
00451     _i2c.write(chip_addr, dt, 1, true);
00452     _i2c.read(chip_addr, dt, 1, false);
00453     return (uint8_t)dt[0];
00454 }
00455 
00456 uint8_t BMX055::write_reg(uint8_t addr, uint8_t data)
00457 {
00458     uint8_t d;
00459 
00460     dt[0] = addr;
00461     dt[1] = data;
00462     _i2c.write(chip_addr, dt, 2, false);
00463     d = dt[0];
00464     return d;
00465 }