altb_pmic
/
AHRS
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Eigen
Embed:
(wiki syntax)
Show/hide line numbers
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 (I2C &i2c) : i2c(i2c) 00027 { 00028 //this->i2c = &i2c; 00029 bmx055_parameters = bmx055_std_paramtr; 00030 initialize (); 00031 } 00032 00033 uint16_t BMX055::begin(void){ 00034 return 1; 00035 } 00036 00037 00038 /////////////// Set parameter ///////////////////////////// 00039 void BMX055::set_parameter(const BMX055_TypeDef *bmx055_parameter) 00040 { 00041 bmx055_parameters = *bmx055_parameter; 00042 set_parameters_to_regs(); 00043 00044 } 00045 00046 /////////////// Read data & normalize ///////////////////// 00047 void BMX055::readAccel(void) 00048 { 00049 int16_t x,y,z; 00050 00051 chip_addr = inf_addr.acc_addr; 00052 dt[0] = 0x02; 00053 i2c.write(chip_addr, dt, 1, true); 00054 i2c.read(chip_addr, dt, 6, false); 00055 x = dt[1] << 8 | (dt[0] & 0xf0); // values have to be divided by 16 00056 y = dt[3] << 8 | (dt[2] & 0xf0); 00057 z = dt[5] << 8 | (dt[4] & 0xf0); 00058 accX = (float)x * acc_factor; 00059 accY = (float)y * acc_factor; 00060 accZ = (float)z * acc_factor; 00061 } 00062 00063 void BMX055::readGyro(void) 00064 { 00065 int16_t x,y,z; 00066 00067 chip_addr = inf_addr.gyr_addr; 00068 dt[0] = 0x02; 00069 i2c.write(chip_addr, dt, 1, true); 00070 i2c.read(chip_addr, dt, 6, false); 00071 x = dt[1] << 8 | dt[0]; 00072 y = dt[3] << 8 | dt[2]; 00073 z = dt[5] << 8 | dt[4]; 00074 00075 gyroX = (float)x * gyr_factor; 00076 gyroY = (float)y * gyr_factor; 00077 gyroZ = (float)z * gyr_factor; 00078 } 00079 00080 void BMX055::readMag(void) 00081 { 00082 int16_t x,y,z; 00083 00084 chip_addr = inf_addr.mag_addr; 00085 dt[0] = 0x42; 00086 i2c.write(chip_addr, dt, 1, true); 00087 i2c.read(chip_addr, dt, 6, false); 00088 00089 // corrected, pmic 21.01.2020 00090 x = dt[1] << 8 | (dt[0] & 0xf8); // value has to be divided by 8 00091 y = dt[3] << 8 | (dt[2] & 0xf8); // value has to be divided by 8 00092 z = dt[5] << 8 | (dt[4] & 0xfe); // value has to be divided by 2 00093 00094 // scaling factor 1/301.5815f if experimentally validated in comparision to pes board -> approx. same scaling of mag values, pmic 21.01.2020 00095 magX = (float)y / 8.0f / 301.5815f; // MAGNETOMETER x-y are switched, see BMX055 datasheet page 161, also measurements show change in sign x!!! 00096 magY = -(float)x / 8.0f / 301.5815f; // x-y is switched 00097 magZ = -(float)z / 2.0f / 301.5815f; // z is -z 00098 } 00099 00100 float BMX055::get_chip_temperature() 00101 { 00102 chip_addr = inf_addr.acc_addr; 00103 dt[0] = 0x08; // chip tempareture reg addr 00104 i2c.write(chip_addr, dt, 1, true); 00105 i2c.read(chip_addr, dt, 1, false); 00106 //printf("Temp reg = 0x%02x\r\n", dt[0]); 00107 return (float)((int8_t)dt[0]) * 0.5f + 23.0f; 00108 } 00109 00110 /////////////// Initialize //////////////////////////////// 00111 void BMX055::initialize (void) 00112 { 00113 i2c.frequency(400000); 00114 // Check Acc & Mag & Gyro are available of not 00115 check_id(); 00116 if (ready_flag == 0x07){ 00117 } 00118 // Set initial data 00119 set_parameters_to_regs(); 00120 switch(bmx055_parameters.gyr_fs){ 00121 case GYR_2000DPS: 00122 gyr_factor = 2000.0f*0.01745329f / 32768.0f; 00123 break; 00124 case GYR_1000DPS: 00125 gyr_factor = 1000.0f*0.01745329f / 32768.0f; 00126 break; 00127 case GYR_500DPS: 00128 gyr_factor = 500.0f*0.01745329f / 32768.0f; 00129 break; 00130 case GYR_250DPS: 00131 gyr_factor = 250.0f*0.01745329f / 32768.0f; 00132 break; 00133 case GYR_125DPS: 00134 gyr_factor = 125.0f*0.01745329f / 32768.0f; 00135 break; 00136 default: 00137 gyr_factor = 0; 00138 break; 00139 } 00140 switch(bmx055_parameters.acc_fs){ 00141 case ACC_2G: 00142 acc_factor = 2.0f * 9.81f / 2048.0f / 16.0f; 00143 break; 00144 case ACC_4G: 00145 acc_factor = 4.0f * 9.81f / 2048.0f / 16.0f; 00146 break; 00147 case ACC_8G: 00148 acc_factor = 8.0f * 9.81f / 2048.0f / 16.0f; 00149 break; 00150 case ACC_16G: 00151 acc_factor = 16.0f * 9.81f / 2048.0f / 16.0f; 00152 break; 00153 default: 00154 acc_factor = 0; 00155 break; 00156 } 00157 00158 00159 } 00160 00161 ////// Set initialize data to related registers /////////// 00162 void BMX055::set_parameters_to_regs(void) 00163 { 00164 // ACC 00165 chip_addr = inf_addr.acc_addr; 00166 dt[0] = 0x0f; // Select PMU_Range register 00167 dt[1] = bmx055_parameters.acc_fs; 00168 i2c.write(chip_addr, dt, 2, false); 00169 wait_ms(1); 00170 dt[0] = 0x10; // Select PMU_BW register 00171 dt[1] = bmx055_parameters.acc_bw; 00172 i2c.write(chip_addr, dt, 2, false); 00173 wait_ms(1); 00174 dt[0] = 0x11; // Select PMU_LPW register 00175 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms 00176 i2c.write(chip_addr, dt, 2, false); 00177 // GYR 00178 chip_addr = inf_addr.gyr_addr; 00179 dt[0] = 0x0f; // Select Range register 00180 dt[1] = bmx055_parameters.gyr_fs; 00181 i2c.write(chip_addr, dt, 2, false); 00182 wait_ms(1); 00183 dt[0] = 0x10; // Select Bandwidth register 00184 dt[1] = bmx055_parameters.gyr_bw; 00185 i2c.write(chip_addr, dt, 2, false); 00186 wait_ms(1); 00187 dt[0] = 0x11; // Select LPM1 register 00188 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms 00189 i2c.write(chip_addr, dt, 2, false); 00190 // MAG 00191 chip_addr = inf_addr.mag_addr; 00192 dt[0] = 0x4b; // Select Mag register 00193 dt[1] = 0x83; // Soft reset 10000011 00194 i2c.write(chip_addr, dt, 2, false); 00195 wait_ms(10); 00196 dt[0] = 0x4b; // Select Mag register 00197 dt[1] = 0x01; // Soft reset 00000001 00198 i2c.write(chip_addr, dt, 2, false); 00199 wait_ms(10); 00200 dt[0] = 0x4c; // Select Mag register 00201 dt[1] = bmx055_parameters.mag_odr << 3; // corrected, pmic 20.01.2020 00202 i2c.write(chip_addr, dt, 2, false); 00203 wait_ms(1); 00204 dt[0] = 0x4e; // Select Mag register 00205 dt[1] = 0x84; // X, Y, Z-Axis enabled 00206 i2c.write(chip_addr, dt, 2, false); 00207 wait_ms(1); 00208 dt[0] = 0x51; // Select Mag register 00209 dt[1] = 0x51; // No. of Repetitions for X-Y Axis = 163, fmaxODR = 33.2060 Hz, pmic 21.01.2020 00210 i2c.write(chip_addr, dt, 2, false); 00211 wait_ms(1); 00212 dt[0] = 0x52; // Select Mag register 00213 dt[1] = 0x0a; // No. of Repetitions for Z-Axis = 11, fmaxODR = 33.2060 Hz, pmic 21.01.2020 00214 i2c.write(chip_addr, dt, 2, false); 00215 #if 0 00216 // ACC 00217 chip_addr = inf_addr.acc_addr; 00218 dt[0] = 0x0f; // Select PMU_Range register 00219 dt[1] = 0x03; // Range = +/- 2g 00220 i2c.write(chip_addr, dt, 2, false); 00221 wait_ms(1); 00222 dt[0] = 0x10; // Select PMU_BW register 00223 dt[1] = 0x08; // Bandwidth = 7.81 Hz 00224 i2c.write(chip_addr, dt, 2, false); 00225 wait_ms(1); 00226 dt[0] = 0x11; // Select PMU_LPW register 00227 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms 00228 i2c.write(chip_addr, dt, 2, false); 00229 wait_ms(1); 00230 // GYR 00231 chip_addr = inf_addr.gyr_addr; 00232 dt[0] = 0x0f; // Select Range register 00233 dt[1] = 0x04; // Full scale = +/- 125 degree/s 00234 i2c.write(chip_addr, dt, 2, false); 00235 wait_ms(1); 00236 dt[0] = 0x10; // Select Bandwidth register 00237 dt[1] = 0x07; // ODR = 100 Hz 00238 i2c.write(chip_addr, dt, 2, false); 00239 wait_ms(1); 00240 dt[0] = 0x11; // Select LPM1 register 00241 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms 00242 i2c.write(chip_addr, dt, 2, false); 00243 // MAG 00244 chip_addr = inf_addr.mag_addr; 00245 dt[0] = 0x4b; // Select Mag register 00246 dt[1] = 0x83; // Soft reset 00247 i2c.write(chip_addr, dt, 2, false); 00248 wait_ms(10); 00249 dt[0] = 0x4b; // Select Mag register 00250 dt[1] = 0x01; // Soft reset 00251 i2c.write(chip_addr, dt, 2, false); 00252 wait_ms(10); 00253 dt[0] = 0x4c; // Select Mag register 00254 dt[1] = 0x00; // Normal Mode, ODR = 10 Hz 00255 i2c.write(chip_addr, dt, 2, false); 00256 wait_ms(1); 00257 dt[0] = 0x4e; // Select Mag register 00258 dt[1] = 0x84; // X, Y, Z-Axis enabled 00259 i2c.write(chip_addr, dt, 2, false); 00260 wait_ms(1); 00261 dt[0] = 0x51; // Select Mag register 00262 dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 00263 i2c.write(chip_addr, dt, 2, false); 00264 wait_ms(1); 00265 dt[0] = 0x52; // Select Mag register 00266 dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 00267 i2c.write(chip_addr, dt, 2, false); 00268 wait_ms(1); 00269 #endif 00270 } 00271 00272 /////////////// Check Who am I? /////////////////////////// 00273 void BMX055::check_id(void) 00274 { 00275 ready_flag = 0; 00276 // ID ACC 00277 inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR; 00278 chip_addr = inf_addr.acc_addr; 00279 dt[0] = 0x00; // chip ID reg addr 00280 i2c.write(chip_addr, dt, 1, true); 00281 i2c.read(chip_addr, dt, 1, false); 00282 inf_id.acc_id = dt[0]; 00283 if (inf_id.acc_id == I_AM_BMX055_ACC) { 00284 ready_flag |= 0x01; 00285 } else { 00286 inf_addr.acc_addr = (0x18 << 1); 00287 chip_addr = inf_addr.acc_addr; 00288 dt[0] = 0x00; // chip ID reg addr 00289 i2c.write(chip_addr, dt, 1, true); 00290 i2c.read(chip_addr, dt, 1, false); 00291 inf_id.acc_id = dt[0]; 00292 if (inf_id.acc_id == I_AM_BMX055_ACC) { 00293 ready_flag |= 0x01; 00294 } 00295 } 00296 // ID GYRO 00297 inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR; 00298 chip_addr = inf_addr.gyr_addr; 00299 dt[0] = 0x00; // chip ID reg addr 00300 i2c.write(chip_addr, dt, 1, true); 00301 i2c.read(chip_addr, dt, 1, false); 00302 inf_id.gyr_id = dt[0]; 00303 if (inf_id.gyr_id == I_AM_BMX055_GYR) { 00304 ready_flag |= 0x02; 00305 } else { 00306 inf_addr.gyr_addr = (0x68 << 1); 00307 chip_addr = inf_addr.gyr_addr; 00308 dt[0] = 0x00; // chip ID reg addr 00309 i2c.write(chip_addr, dt, 1, true); 00310 i2c.read(chip_addr, dt, 1, false); 00311 inf_id.gyr_id = dt[0]; 00312 if (inf_id.gyr_id == I_AM_BMX055_GYR) { 00313 ready_flag |= 0x02; 00314 } 00315 } 00316 // ID Mag 00317 inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR; 00318 chip_addr = inf_addr.mag_addr; 00319 dt[0] = 0x4b; // reg addr 00320 dt[1] = 0x01; // control power bit set 1 00321 i2c.write(chip_addr, dt, 2, false); 00322 dt[0] = 0x40; // chip ID reg addr 00323 i2c.write(chip_addr, dt, 1, true); 00324 i2c.read(chip_addr, dt, 1, false); 00325 inf_id.mag_id = dt[0]; 00326 if (inf_id.mag_id == I_AM_BMX055_MAG) { 00327 ready_flag |= 0x04; 00328 } else { 00329 inf_addr.mag_addr = (0x12 << 1); 00330 chip_addr = inf_addr.mag_addr; 00331 // control power bit set 1 00332 dt[0] = 0x4b; 00333 dt[1] = 0x01; 00334 i2c.write(chip_addr, dt, 2, false); 00335 dt[0] = 0x40; 00336 i2c.write(chip_addr, dt, 1, true); 00337 i2c.read(chip_addr, dt, 1, false); 00338 inf_id.mag_id = dt[0]; 00339 if (inf_id.mag_id == I_AM_BMX055_MAG) { 00340 ready_flag |= 0x04; 00341 } else { 00342 inf_addr.mag_addr = (0x11 << 1); 00343 chip_addr = inf_addr.mag_addr; 00344 // control power bit set 1 00345 dt[0] = 0x4b; 00346 dt[1] = 0x01; 00347 i2c.write(chip_addr, dt, 2, false); 00348 dt[0] = 0x40; 00349 i2c.write(chip_addr, dt, 1, true); 00350 i2c.read(chip_addr, dt, 1, false); 00351 inf_id.mag_id = dt[0]; 00352 if (inf_id.mag_id == I_AM_BMX055_MAG) { 00353 ready_flag |= 0x04; 00354 } else { 00355 inf_addr.mag_addr = (0x10 << 1); 00356 chip_addr = inf_addr.mag_addr; 00357 // control power bit set 1 00358 dt[0] = 0x4b; 00359 dt[1] = 0x01; 00360 i2c.write(chip_addr, dt, 2, false); 00361 dt[0] = 0x40; 00362 i2c.write(chip_addr, dt, 1, true); 00363 i2c.read(chip_addr, dt, 1, false); 00364 inf_id.mag_id = dt[0]; 00365 if (inf_id.mag_id == I_AM_BMX055_MAG) { 00366 ready_flag |= 0x04; 00367 } 00368 } 00369 } 00370 } 00371 #if DEBUG 00372 printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id); 00373 printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id); 00374 printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id); 00375 printf("ready_flag = 0x%x\r\n", ready_flag); 00376 #endif 00377 } 00378 00379 void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id) 00380 { 00381 id->acc_id = acc_id; 00382 id->mag_id = mag_id; 00383 id->gyr_id = gyr_id; 00384 } 00385 00386 /////////////// Check chip ready or not ////////////////// 00387 bool BMX055::chip_ready(void) 00388 { 00389 if (ready_flag == 0x07) { 00390 return true; 00391 } 00392 return false; 00393 } 00394 00395 /////////////// I2C Freq. ///////////////////////////////// 00396 void BMX055::frequency(int hz) 00397 { 00398 i2c.frequency(hz); 00399 } 00400 00401 /////////////// Read/Write specific register ////////////// 00402 uint8_t BMX055::read_reg(uint8_t addr) 00403 { 00404 dt[0] = addr; 00405 i2c.write(chip_addr, dt, 1, true); 00406 i2c.read(chip_addr, dt, 1, false); 00407 return (uint8_t)dt[0]; 00408 } 00409 00410 uint8_t BMX055::write_reg(uint8_t addr, uint8_t data) 00411 { 00412 uint8_t d; 00413 00414 dt[0] = addr; 00415 dt[1] = data; 00416 i2c.write(chip_addr, dt, 2, false); 00417 d = dt[0]; 00418 return d; 00419 }
Generated on Thu Jul 14 2022 22:08:52 by
1.7.2