Andrew SaLoutos
/
ICM_20948
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.
Embed:
(wiki syntax)
Show/hide line numbers
ICM_20948.cpp
00001 #include "ICM_20948.h" 00002 00003 #include "util/ICM_20948_REGISTERS.h" 00004 #include "util/AK09916_REGISTERS.h" 00005 00006 // Forward Declarations 00007 ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); 00008 ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); 00009 00010 // Base 00011 ICM_20948::ICM_20948() 00012 { 00013 status = ICM_20948_init_struct(&_device); 00014 } 00015 00016 void ICM_20948::enableDebugging(Serial &debugPort) 00017 { 00018 _debugSerial = &debugPort; //Grab which port the user wants us to use for debugging 00019 _printDebug = true; //Should we print the commands we send? Good for debugging 00020 } 00021 void ICM_20948::disableDebugging(void) 00022 { 00023 _printDebug = false; //Turn off extra print statements 00024 } 00025 00026 void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat) 00027 { 00028 // if (_printDebug == true) { 00029 switch (stat) 00030 { 00031 case ICM_20948_Stat_Ok: 00032 _debugSerial->printf("All is well.\n\r"); 00033 break; 00034 case ICM_20948_Stat_Err: 00035 _debugSerial->printf("General Error\n\r"); 00036 break; 00037 case ICM_20948_Stat_NotImpl: 00038 _debugSerial->printf("Not Implemented\n\r"); 00039 break; 00040 case ICM_20948_Stat_ParamErr: 00041 _debugSerial->printf("Parameter Error\n\r"); 00042 break; 00043 case ICM_20948_Stat_WrongID: 00044 _debugSerial->printf("Wrong ID\n\r"); 00045 break; 00046 case ICM_20948_Stat_InvalSensor: 00047 _debugSerial->printf("Invalid Sensor\n\r"); 00048 break; 00049 case ICM_20948_Stat_NoData: 00050 _debugSerial->printf("Data Underflow\n\r"); 00051 break; 00052 case ICM_20948_Stat_SensorNotSupported: 00053 _debugSerial->printf("Sensor Not Supported\n\r"); 00054 break; 00055 case ICM_20948_Stat_DMPNotSupported: 00056 _debugSerial->printf("DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?\n\r"); 00057 break; 00058 case ICM_20948_Stat_DMPVerifyFail: 00059 _debugSerial->printf("DMP Firmware Verification Failed\n\r"); 00060 break; 00061 case ICM_20948_Stat_FIFONoDataAvail: 00062 _debugSerial->printf("No FIFO Data Available\n\r"); 00063 break; 00064 case ICM_20948_Stat_FIFOIncompleteData: 00065 _debugSerial->printf("DMP data in FIFO was incomplete\n\r"); 00066 break; 00067 case ICM_20948_Stat_FIFOMoreDataAvail: 00068 _debugSerial->printf("More FIFO Data Available\n\r"); 00069 break; 00070 case ICM_20948_Stat_UnrecognisedDMPHeader: 00071 _debugSerial->printf("Unrecognised DMP Header\n\r"); 00072 break; 00073 case ICM_20948_Stat_UnrecognisedDMPHeader2: 00074 _debugSerial->printf("Unrecognised DMP Header2\n\r"); 00075 break; 00076 case ICM_20948_Stat_InvalDMPRegister: 00077 _debugSerial->printf("Invalid DMP Register\n\r"); 00078 break; 00079 default: 00080 _debugSerial->printf("Unknown Status\n\r"); 00081 break; 00082 } 00083 // } // end if debugging==true 00084 } 00085 00086 ICM_20948_AGMT_t ICM_20948::getAGMT(void) 00087 { 00088 status = ICM_20948_get_agmt(&_device, &agmt); 00089 00090 return agmt; 00091 } 00092 00093 float ICM_20948::magX(void) 00094 { 00095 return getMagUT(agmt.mag.axes.x); 00096 } 00097 00098 float ICM_20948::magY(void) 00099 { 00100 return getMagUT(agmt.mag.axes.y); 00101 } 00102 00103 float ICM_20948::magZ(void) 00104 { 00105 return getMagUT(agmt.mag.axes.z); 00106 } 00107 00108 float ICM_20948::getMagUT(int16_t axis_val) 00109 { 00110 return (((float)axis_val) * 0.15); 00111 } 00112 00113 float ICM_20948::accX(void) 00114 { 00115 return getAccMG(agmt.acc.axes.x); 00116 } 00117 00118 float ICM_20948::accY(void) 00119 { 00120 return getAccMG(agmt.acc.axes.y); 00121 } 00122 00123 float ICM_20948::accZ(void) 00124 { 00125 return getAccMG(agmt.acc.axes.z); 00126 } 00127 00128 float ICM_20948::getAccMG(int16_t axis_val) 00129 { 00130 switch (agmt.fss.a) 00131 { 00132 case 0: 00133 return (((float)axis_val) / 16.384); 00134 break; 00135 case 1: 00136 return (((float)axis_val) / 8.192); 00137 break; 00138 case 2: 00139 return (((float)axis_val) / 4.096); 00140 break; 00141 case 3: 00142 return (((float)axis_val) / 2.048); 00143 break; 00144 default: 00145 return 0; 00146 break; 00147 } 00148 } 00149 00150 float ICM_20948::gyrX(void) 00151 { 00152 return getGyrDPS(agmt.gyr.axes.x); 00153 } 00154 00155 float ICM_20948::gyrY(void) 00156 { 00157 return getGyrDPS(agmt.gyr.axes.y); 00158 } 00159 00160 float ICM_20948::gyrZ(void) 00161 { 00162 return getGyrDPS(agmt.gyr.axes.z); 00163 } 00164 00165 float ICM_20948::getGyrDPS(int16_t axis_val) 00166 { 00167 switch (agmt.fss.g) 00168 { 00169 case 0: 00170 return (((float)axis_val) / 131); 00171 break; 00172 case 1: 00173 return (((float)axis_val) / 65.5); 00174 break; 00175 case 2: 00176 return (((float)axis_val) / 32.8); 00177 break; 00178 case 3: 00179 return (((float)axis_val) / 16.4); 00180 break; 00181 default: 00182 return 0; 00183 break; 00184 } 00185 } 00186 00187 float ICM_20948::temp(void) 00188 { 00189 return getTempC(agmt.tmp.val); 00190 } 00191 00192 float ICM_20948::getTempC(int16_t val) 00193 { 00194 return (((float)val - 21) / 333.87) + 21; 00195 } 00196 00197 // Device Level 00198 ICM_20948_Status_e ICM_20948::setBank(uint8_t bank) 00199 { 00200 status = ICM_20948_set_bank(&_device, bank); 00201 return status; 00202 } 00203 00204 ICM_20948_Status_e ICM_20948::swReset(void) 00205 { 00206 status = ICM_20948_sw_reset(&_device); 00207 return status; 00208 } 00209 00210 ICM_20948_Status_e ICM_20948::sleep(bool on) 00211 { 00212 status = ICM_20948_sleep(&_device, on); 00213 return status; 00214 } 00215 00216 ICM_20948_Status_e ICM_20948::lowPower(bool on) 00217 { 00218 status = ICM_20948_low_power(&_device, on); 00219 return status; 00220 } 00221 00222 ICM_20948_Status_e ICM_20948::setClockSource(ICM_20948_PWR_MGMT_1_CLKSEL_e source) 00223 { 00224 status = ICM_20948_set_clock_source(&_device, source); 00225 return status; 00226 } 00227 00228 ICM_20948_Status_e ICM_20948::checkID(void) 00229 { 00230 status = ICM_20948_check_id(&_device); 00231 if (status != ICM_20948_Stat_Ok) 00232 { 00233 if (_printDebug) { 00234 _debugSerial->printf("ICM_20948::checkID: ICM_20948_check_id returned: "); 00235 debugPrintStatus(status); 00236 } 00237 } 00238 return status; 00239 } 00240 00241 bool ICM_20948::dataReady(void) 00242 { 00243 status = ICM_20948_data_ready(&_device); 00244 if (status == ICM_20948_Stat_Ok) 00245 { 00246 return true; 00247 } 00248 return false; 00249 } 00250 00251 uint8_t ICM_20948::getWhoAmI(void) 00252 { 00253 uint8_t retval = 0x00; 00254 status = ICM_20948_get_who_am_i(&_device, &retval); 00255 return retval; 00256 } 00257 00258 bool ICM_20948::isConnected(void) 00259 { 00260 status = checkID(); 00261 if (status == ICM_20948_Stat_Ok) 00262 { 00263 return true; 00264 } 00265 if (_printDebug) { 00266 _debugSerial->printf("ICM_20948::isConnected: checkID returned: "); 00267 debugPrintStatus(status); 00268 } 00269 return false; 00270 } 00271 00272 // Internal Sensor Options 00273 ICM_20948_Status_e ICM_20948::setSampleMode(uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode) 00274 { 00275 status = ICM_20948_set_sample_mode(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode); 00276 wait_ms(1); // Give the ICM20948 time to change the sample mode (see issue #8) 00277 return status; 00278 } 00279 00280 ICM_20948_Status_e ICM_20948::setFullScale(uint8_t sensor_id_bm, ICM_20948_fss_t fss) 00281 { 00282 status = ICM_20948_set_full_scale(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, fss); 00283 return status; 00284 } 00285 00286 ICM_20948_Status_e ICM_20948::setDLPFcfg(uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg) 00287 { 00288 status = ICM_20948_set_dlpf_cfg(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, cfg); 00289 return status; 00290 } 00291 00292 ICM_20948_Status_e ICM_20948::enableDLPF(uint8_t sensor_id_bm, bool enable) 00293 { 00294 status = ICM_20948_enable_dlpf(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, enable); 00295 return status; 00296 } 00297 00298 ICM_20948_Status_e ICM_20948::setSampleRate(uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt) 00299 { 00300 status = ICM_20948_set_sample_rate(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, smplrt); 00301 return status; 00302 } 00303 00304 // Interrupts on INT Pin 00305 ICM_20948_Status_e ICM_20948::clearInterrupts(void) 00306 { 00307 ICM_20948_INT_STATUS_t int_stat; 00308 ICM_20948_INT_STATUS_1_t int_stat_1; 00309 00310 // read to clear interrupts 00311 status = ICM_20948_set_bank(&_device, 0); 00312 if (status != ICM_20948_Stat_Ok) 00313 { 00314 return status; 00315 } 00316 status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t)); 00317 if (status != ICM_20948_Stat_Ok) 00318 { 00319 return status; 00320 } 00321 status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS_1, (uint8_t *)&int_stat_1, sizeof(ICM_20948_INT_STATUS_1_t)); 00322 if (status != ICM_20948_Stat_Ok) 00323 { 00324 return status; 00325 } 00326 00327 // todo: there may be additional interrupts that need to be cleared, like FIFO overflow/watermark 00328 00329 return status; 00330 } 00331 00332 ICM_20948_Status_e ICM_20948::cfgIntActiveLow(bool active_low) 00333 { 00334 ICM_20948_INT_PIN_CFG_t reg; 00335 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00336 if (status != ICM_20948_Stat_Ok) 00337 { 00338 return status; 00339 } 00340 reg.INT1_ACTL = active_low; // set the setting 00341 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00342 if (status != ICM_20948_Stat_Ok) 00343 { 00344 return status; 00345 } 00346 return status; 00347 } 00348 00349 ICM_20948_Status_e ICM_20948::cfgIntOpenDrain(bool open_drain) 00350 { 00351 ICM_20948_INT_PIN_CFG_t reg; 00352 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00353 if (status != ICM_20948_Stat_Ok) 00354 { 00355 return status; 00356 } 00357 reg.INT1_OPEN = open_drain; // set the setting 00358 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00359 if (status != ICM_20948_Stat_Ok) 00360 { 00361 return status; 00362 } 00363 return status; 00364 } 00365 00366 ICM_20948_Status_e ICM_20948::cfgIntLatch(bool latching) 00367 { 00368 ICM_20948_INT_PIN_CFG_t reg; 00369 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00370 if (status != ICM_20948_Stat_Ok) 00371 { 00372 return status; 00373 } 00374 reg.INT1_LATCH_EN = latching; // set the setting 00375 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00376 if (status != ICM_20948_Stat_Ok) 00377 { 00378 return status; 00379 } 00380 return status; 00381 } 00382 00383 ICM_20948_Status_e ICM_20948::cfgIntAnyReadToClear(bool enabled) 00384 { 00385 ICM_20948_INT_PIN_CFG_t reg; 00386 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00387 if (status != ICM_20948_Stat_Ok) 00388 { 00389 return status; 00390 } 00391 reg.INT_ANYRD_2CLEAR = enabled; // set the setting 00392 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00393 if (status != ICM_20948_Stat_Ok) 00394 { 00395 return status; 00396 } 00397 return status; 00398 } 00399 00400 ICM_20948_Status_e ICM_20948::cfgFsyncActiveLow(bool active_low) 00401 { 00402 ICM_20948_INT_PIN_CFG_t reg; 00403 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00404 if (status != ICM_20948_Stat_Ok) 00405 { 00406 return status; 00407 } 00408 reg.ACTL_FSYNC = active_low; // set the setting 00409 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00410 if (status != ICM_20948_Stat_Ok) 00411 { 00412 return status; 00413 } 00414 return status; 00415 } 00416 00417 ICM_20948_Status_e ICM_20948::cfgFsyncIntMode(bool interrupt_mode) 00418 { 00419 ICM_20948_INT_PIN_CFG_t reg; 00420 status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase 00421 if (status != ICM_20948_Stat_Ok) 00422 { 00423 return status; 00424 } 00425 reg.FSYNC_INT_MODE_EN = interrupt_mode; // set the setting 00426 status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase 00427 if (status != ICM_20948_Stat_Ok) 00428 { 00429 return status; 00430 } 00431 return status; 00432 } 00433 00434 // All these individual functions will use a read->set->write method to leave other settings untouched 00435 ICM_20948_Status_e ICM_20948::intEnableI2C(bool enable) 00436 { 00437 ICM_20948_INT_enable_t en; // storage 00438 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00439 if (status != ICM_20948_Stat_Ok) 00440 { 00441 return status; 00442 } 00443 en.I2C_MST_INT_EN = enable; // change the setting 00444 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00445 if (status != ICM_20948_Stat_Ok) 00446 { 00447 return status; 00448 } 00449 if (en.I2C_MST_INT_EN != enable) 00450 { 00451 status = ICM_20948_Stat_Err; 00452 return status; 00453 } 00454 return status; 00455 } 00456 00457 ICM_20948_Status_e ICM_20948::intEnableDMP(bool enable) 00458 { 00459 ICM_20948_INT_enable_t en; // storage 00460 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00461 if (status != ICM_20948_Stat_Ok) 00462 { 00463 return status; 00464 } 00465 en.DMP_INT1_EN = enable; // change the setting 00466 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00467 if (status != ICM_20948_Stat_Ok) 00468 { 00469 return status; 00470 } 00471 if (en.DMP_INT1_EN != enable) 00472 { 00473 status = ICM_20948_Stat_Err; 00474 return status; 00475 } 00476 return status; 00477 } 00478 00479 ICM_20948_Status_e ICM_20948::intEnablePLL(bool enable) 00480 { 00481 ICM_20948_INT_enable_t en; // storage 00482 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00483 if (status != ICM_20948_Stat_Ok) 00484 { 00485 return status; 00486 } 00487 en.PLL_RDY_EN = enable; // change the setting 00488 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00489 if (status != ICM_20948_Stat_Ok) 00490 { 00491 return status; 00492 } 00493 if (en.PLL_RDY_EN != enable) 00494 { 00495 status = ICM_20948_Stat_Err; 00496 return status; 00497 } 00498 return status; 00499 } 00500 00501 ICM_20948_Status_e ICM_20948::intEnableWOM(bool enable) 00502 { 00503 ICM_20948_INT_enable_t en; // storage 00504 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00505 if (status != ICM_20948_Stat_Ok) 00506 { 00507 return status; 00508 } 00509 en.WOM_INT_EN = enable; // change the setting 00510 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00511 if (status != ICM_20948_Stat_Ok) 00512 { 00513 return status; 00514 } 00515 if (en.WOM_INT_EN != enable) 00516 { 00517 status = ICM_20948_Stat_Err; 00518 return status; 00519 } 00520 return status; 00521 } 00522 00523 ICM_20948_Status_e ICM_20948::intEnableWOF(bool enable) 00524 { 00525 ICM_20948_INT_enable_t en; // storage 00526 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00527 if (status != ICM_20948_Stat_Ok) 00528 { 00529 return status; 00530 } 00531 en.REG_WOF_EN = enable; // change the setting 00532 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00533 if (status != ICM_20948_Stat_Ok) 00534 { 00535 return status; 00536 } 00537 if (en.REG_WOF_EN != enable) 00538 { 00539 status = ICM_20948_Stat_Err; 00540 return status; 00541 } 00542 return status; 00543 } 00544 00545 ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable) 00546 { 00547 ICM_20948_INT_enable_t en; // storage 00548 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00549 if (status != ICM_20948_Stat_Ok) 00550 { 00551 return status; 00552 } 00553 en.RAW_DATA_0_RDY_EN = enable; // change the setting 00554 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00555 if (status != ICM_20948_Stat_Ok) 00556 { 00557 return status; 00558 } 00559 if (en.RAW_DATA_0_RDY_EN != enable) 00560 { 00561 status = ICM_20948_Stat_Err; 00562 return status; 00563 } 00564 return status; 00565 } 00566 00567 ICM_20948_Status_e ICM_20948::intEnableOverflowFIFO(uint8_t bm_enable) 00568 { 00569 ICM_20948_INT_enable_t en; // storage 00570 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00571 if (status != ICM_20948_Stat_Ok) 00572 { 00573 return status; 00574 } 00575 en.FIFO_OVERFLOW_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings 00576 en.FIFO_OVERFLOW_EN_1 = ((bm_enable >> 1) & 0x01); 00577 en.FIFO_OVERFLOW_EN_2 = ((bm_enable >> 2) & 0x01); 00578 en.FIFO_OVERFLOW_EN_3 = ((bm_enable >> 3) & 0x01); 00579 en.FIFO_OVERFLOW_EN_4 = ((bm_enable >> 4) & 0x01); 00580 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00581 if (status != ICM_20948_Stat_Ok) 00582 { 00583 return status; 00584 } 00585 return status; 00586 } 00587 00588 ICM_20948_Status_e ICM_20948::intEnableWatermarkFIFO(uint8_t bm_enable) 00589 { 00590 ICM_20948_INT_enable_t en; // storage 00591 status = ICM_20948_int_enable(&_device, NULL, &en); // read phase 00592 if (status != ICM_20948_Stat_Ok) 00593 { 00594 return status; 00595 } 00596 en.FIFO_WM_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings 00597 en.FIFO_WM_EN_1 = ((bm_enable >> 1) & 0x01); 00598 en.FIFO_WM_EN_2 = ((bm_enable >> 2) & 0x01); 00599 en.FIFO_WM_EN_3 = ((bm_enable >> 3) & 0x01); 00600 en.FIFO_WM_EN_4 = ((bm_enable >> 4) & 0x01); 00601 status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback 00602 if (status != ICM_20948_Stat_Ok) 00603 { 00604 return status; 00605 } 00606 return status; 00607 } 00608 00609 ICM_20948_Status_e ICM_20948::WOMThreshold(uint8_t threshold) 00610 { 00611 ICM_20948_ACCEL_WOM_THR_t thr; // storage 00612 status = ICM_20948_wom_threshold(&_device, NULL, &thr); // read phase 00613 if (status != ICM_20948_Stat_Ok) 00614 { 00615 return status; 00616 } 00617 thr.WOM_THRESHOLD = threshold; // change the setting 00618 status = ICM_20948_wom_threshold(&_device, &thr, &thr); // write phase w/ readback 00619 if (status != ICM_20948_Stat_Ok) 00620 { 00621 return status; 00622 } 00623 if (thr.WOM_THRESHOLD != threshold) 00624 { 00625 status = ICM_20948_Stat_Err; 00626 return status; 00627 } 00628 return status; 00629 } 00630 00631 // Interface Options 00632 ICM_20948_Status_e ICM_20948::i2cMasterPassthrough(bool passthrough) 00633 { 00634 status = ICM_20948_i2c_master_passthrough(&_device, passthrough); 00635 return status; 00636 } 00637 00638 ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable) 00639 { 00640 status = ICM_20948_i2c_master_enable(&_device, enable); 00641 return status; 00642 } 00643 00644 ICM_20948_Status_e ICM_20948::i2cMasterReset() 00645 { 00646 status = ICM_20948_i2c_master_reset(&_device); 00647 return status; 00648 } 00649 00650 ICM_20948_Status_e ICM_20948::i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut) 00651 { 00652 status = ICM_20948_i2c_controller_configure_peripheral(&_device, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut); 00653 return status; 00654 } 00655 00656 ICM_20948_Status_e ICM_20948::i2cControllerPeriph4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr) 00657 { 00658 status = ICM_20948_i2c_controller_periph4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr); 00659 return status; 00660 } 00661 00662 ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data) 00663 { 00664 status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data); 00665 return status; 00666 } 00667 uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg) 00668 { 00669 uint8_t data = 0; 00670 status = ICM_20948_i2c_master_single_r(&_device, addr, reg, &data); 00671 if (status != ICM_20948_Stat_Ok) 00672 { 00673 if (_printDebug) { 00674 _debugSerial->printf("ICM_20948::i2cMasterSingleR: ICM_20948_i2c_master_single_r returned: "); 00675 debugPrintStatus(status); 00676 } 00677 } 00678 return data; 00679 } 00680 00681 ICM_20948_Status_e ICM_20948::startupDefault(bool minimal) 00682 { 00683 ICM_20948_Status_e retval = ICM_20948_Stat_Ok; 00684 00685 retval = checkID(); 00686 if (retval != ICM_20948_Stat_Ok) 00687 { 00688 if (_printDebug) { 00689 _debugSerial->printf("ICM_20948::startupDefault: checkID returned: "); 00690 debugPrintStatus(retval); 00691 } 00692 status = retval; 00693 return status; 00694 } 00695 00696 retval = swReset(); 00697 if (retval != ICM_20948_Stat_Ok) 00698 { 00699 if (_printDebug) { 00700 _debugSerial->printf("ICM_20948::startupDefault: swReset returned: "); 00701 debugPrintStatus(retval); 00702 } 00703 status = retval; 00704 return status; 00705 } 00706 wait_ms(50); 00707 00708 retval = sleep(false); 00709 if (retval != ICM_20948_Stat_Ok) 00710 { 00711 if (_printDebug) { 00712 _debugSerial->printf("ICM_20948::startupDefault: sleep returned: "); 00713 debugPrintStatus(retval); 00714 } 00715 status = retval; 00716 return status; 00717 } 00718 00719 retval = lowPower(false); 00720 if (retval != ICM_20948_Stat_Ok) 00721 { 00722 if (_printDebug) { 00723 _debugSerial->printf("ICM_20948::startupDefault: lowPower returned: "); 00724 debugPrintStatus(retval); 00725 } 00726 status = retval; 00727 return status; 00728 } 00729 00730 retval = startupMagnetometer(minimal); // Pass the minimal startup flag to startupMagnetometer 00731 if (retval != ICM_20948_Stat_Ok) 00732 { 00733 if (_printDebug) { 00734 _debugSerial->printf("ICM_20948::startupDefault: startupMagnetometer returned: "); 00735 debugPrintStatus(retval); 00736 } 00737 status = retval; 00738 return status; 00739 } 00740 00741 if (minimal) // Return now if minimal is true 00742 { 00743 if (_printDebug) { 00744 _debugSerial->printf("ICM_20948::startupDefault: minimal startup complete!"); 00745 } 00746 return status; 00747 } 00748 00749 retval = setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled 00750 if (retval != ICM_20948_Stat_Ok) 00751 { 00752 if (_printDebug) { 00753 _debugSerial->printf("ICM_20948::startupDefault: setSampleMode returned: "); 00754 debugPrintStatus(retval); 00755 } 00756 status = retval; 00757 return status; 00758 } // sensors: ICM_20948_Internal_Acc, ICM_20948_Internal_Gyr, ICM_20948_Internal_Mst 00759 00760 ICM_20948_fss_t FSS; 00761 FSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) 00762 FSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) 00763 retval = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS); 00764 if (retval != ICM_20948_Stat_Ok) 00765 { 00766 if (_printDebug) { 00767 _debugSerial->printf("ICM_20948::startupDefault: setFullScale returned: "); 00768 debugPrintStatus(retval); 00769 } 00770 status = retval; 00771 return status; 00772 } 00773 00774 ICM_20948_dlpcfg_t dlpcfg; 00775 dlpcfg.a = acc_d473bw_n499bw; 00776 dlpcfg.g = gyr_d361bw4_n376bw5; 00777 retval = setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg); 00778 if (retval != ICM_20948_Stat_Ok) 00779 { 00780 if (_printDebug) { 00781 _debugSerial->printf("ICM_20948::startupDefault: setDLPFcfg returned: "); 00782 debugPrintStatus(retval); 00783 } 00784 status = retval; 00785 return status; 00786 } 00787 00788 retval = enableDLPF(ICM_20948_Internal_Acc, false); 00789 if (retval != ICM_20948_Stat_Ok) 00790 { 00791 if (_printDebug) { 00792 _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Acc) returned: "); 00793 debugPrintStatus(retval); 00794 } 00795 status = retval; 00796 return status; 00797 } 00798 00799 retval = enableDLPF(ICM_20948_Internal_Gyr, false); 00800 if (retval != ICM_20948_Stat_Ok) 00801 { 00802 if (_printDebug) { 00803 _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Gyr) returned: "); 00804 debugPrintStatus(retval); 00805 } 00806 status = retval; 00807 return status; 00808 } 00809 00810 return status; 00811 } 00812 00813 // direct read/write 00814 ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len) 00815 { 00816 status = ICM_20948_execute_r(&_device, reg, pdata, len); 00817 return (status); 00818 } 00819 00820 ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len) 00821 { 00822 status = ICM_20948_execute_w(&_device, reg, pdata, len); 00823 return (status); 00824 } 00825 00826 uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg) 00827 { 00828 uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg); // i2cMasterSingleR updates status too 00829 return data; 00830 } 00831 00832 ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata) 00833 { 00834 status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, *pdata); 00835 return status; 00836 } 00837 00838 ICM_20948_Status_e ICM_20948::resetMag() 00839 { 00840 uint8_t SRST = 1; 00841 // SRST: Soft reset 00842 // “0”: Normal 00843 // “1”: Reset 00844 // When “1” is set, all registers are initialized. After reset, SRST bit turns to “0” automatically. 00845 status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL3, SRST); 00846 return status; 00847 } 00848 00849 // FIFO 00850 00851 ICM_20948_Status_e ICM_20948::enableFIFO(bool enable) 00852 { 00853 status = ICM_20948_enable_FIFO(&_device, enable); 00854 return status; 00855 } 00856 00857 ICM_20948_Status_e ICM_20948::resetFIFO(void) 00858 { 00859 status = ICM_20948_reset_FIFO(&_device); 00860 return status; 00861 } 00862 00863 ICM_20948_Status_e ICM_20948::setFIFOmode(bool snapshot) 00864 { 00865 // Default to Stream (non-Snapshot) mode 00866 status = ICM_20948_set_FIFO_mode(&_device, snapshot); 00867 return status; 00868 } 00869 00870 ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count) 00871 { 00872 status = ICM_20948_get_FIFO_count(&_device, count); 00873 return status; 00874 } 00875 00876 ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data, uint8_t len) 00877 { 00878 status = ICM_20948_read_FIFO(&_device, data, len); 00879 return status; 00880 } 00881 00882 // DMP 00883 00884 ICM_20948_Status_e ICM_20948::enableDMP(bool enable) 00885 { 00886 if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP? 00887 { 00888 status = ICM_20948_enable_DMP(&_device, enable == true ? 1 : 0); 00889 return status; 00890 } 00891 return ICM_20948_Stat_DMPNotSupported; 00892 } 00893 00894 ICM_20948_Status_e ICM_20948::resetDMP(void) 00895 { 00896 status = ICM_20948_reset_DMP(&_device); 00897 return status; 00898 } 00899 00900 ICM_20948_Status_e ICM_20948::loadDMPFirmware(void) 00901 { 00902 if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware? 00903 { 00904 status = ICM_20948_firmware_load(&_device); 00905 return status; 00906 } 00907 return ICM_20948_Stat_DMPNotSupported; 00908 } 00909 00910 ICM_20948_Status_e ICM_20948::setDMPstartAddress(unsigned short address) 00911 { 00912 if (_device._dmp_firmware_available == true) // Should we attempt to set the start address? 00913 { 00914 status = ICM_20948_set_dmp_start_address(&_device, address); 00915 return status; 00916 } 00917 return ICM_20948_Stat_DMPNotSupported; 00918 } 00919 00920 ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable) 00921 { 00922 if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor? 00923 { 00924 status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0); 00925 if (_printDebug) { 00926 _debugSerial->printf("ICM_20948::enableDMPSensor: _enabled_Android_0: %d", (int)_device._enabled_Android_0); 00927 _debugSerial->printf(" _enabled_Android_1: %d", (int)_device._enabled_Android_1); 00928 _debugSerial->printf(" _dataOutCtl1: %d", (int)_device._dataOutCtl1); 00929 _debugSerial->printf(" _dataOutCtl2: %d", (int)_device._dataOutCtl2); 00930 _debugSerial->printf(" _dataRdyStatus: %d\n\r", (int)_device._dataRdyStatus); 00931 } 00932 return status; 00933 } 00934 return ICM_20948_Stat_DMPNotSupported; 00935 } 00936 00937 ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable) 00938 { 00939 if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor interrupt? 00940 { 00941 status = inv_icm20948_enable_dmp_sensor_int(&_device, sensor, enable == true ? 1 : 0); 00942 if (_printDebug) { 00943 _debugSerial->printf("ICM_20948::enableDMPSensorInt: _enabled_Android_intr_0: %d", (int)_device._enabled_Android_intr_0); 00944 _debugSerial->printf(" _enabled_Android_intr_1: %d", (int)_device._enabled_Android_intr_1); 00945 _debugSerial->printf(" _dataIntrCtl: %d\n\r", (int)_device._dataIntrCtl); 00946 } 00947 return status; 00948 } 00949 return ICM_20948_Stat_DMPNotSupported; 00950 } 00951 00952 ICM_20948_Status_e ICM_20948::writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data) 00953 { 00954 if (_device._dmp_firmware_available == true) // Should we attempt to write to the DMP? 00955 { 00956 status = inv_icm20948_write_mems(&_device, reg, length, data); 00957 return status; 00958 } 00959 return ICM_20948_Stat_DMPNotSupported; 00960 } 00961 00962 ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int length, unsigned char *data) 00963 { 00964 if (_device._dmp_firmware_available == true) // Should we attempt to read from the DMP? 00965 { 00966 status = inv_icm20948_read_mems(&_device, reg, length, data); 00967 return status; 00968 } 00969 return ICM_20948_Stat_DMPNotSupported; 00970 } 00971 00972 ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval) 00973 { 00974 if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR? 00975 { 00976 // In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor. 00977 // Setting value can be calculated as follows: 00978 // Value = (DMP running rate (225Hz) / ODR ) - 1 00979 // E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8. 00980 00981 status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval); 00982 return status; 00983 } 00984 return ICM_20948_Stat_DMPNotSupported; 00985 } 00986 00987 ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data) 00988 { 00989 if (_device._dmp_firmware_available == true) // Should we attempt to set the data from the FIFO? 00990 { 00991 status = inv_icm20948_read_dmp_data(&_device, data); 00992 return status; 00993 } 00994 return ICM_20948_Stat_DMPNotSupported; 00995 } 00996 00997 ICM_20948_Status_e ICM_20948::setGyroSF(unsigned char div, int gyro_level) 00998 { 00999 if (_device._dmp_firmware_available == true) // Should we attempt to set the Gyro SF? 01000 { 01001 status = inv_icm20948_set_gyro_sf(&_device, div, gyro_level); 01002 if (_printDebug) { 01003 _debugSerial->printf("ICM_20948::setGyroSF: pll: %d", (int)_device._gyroSFpll); 01004 _debugSerial->printf(" Gyro SF is: %d\n\r", (int)_device._gyroSF); 01005 } 01006 return status; 01007 } 01008 return ICM_20948_Stat_DMPNotSupported; 01009 } 01010 01011 // Combine all of the DMP start-up code from the earlier DMP examples 01012 // This function is defined as __attribute__((weak)) so you can overwrite it if you want to, 01013 // e.g. to modify the sample rate 01014 ICM_20948_Status_e ICM_20948::initializeDMP(void) 01015 { 01016 // First, let's check if the DMP is available 01017 if (_device._dmp_firmware_available != true) 01018 { 01019 if (_printDebug) { 01020 _debugSerial->printf("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...\n\r"); 01021 } 01022 return ICM_20948_Stat_DMPNotSupported; 01023 } 01024 01025 ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; 01026 01027 #if defined(ICM_20948_USE_DMP) 01028 01029 // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration 01030 // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions". 01031 01032 ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful 01033 01034 // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer 01035 // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read 01036 // nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. 01037 // Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit. 01038 // 01039 // But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, 01040 // when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved 01041 // RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here... 01042 // The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04. 01043 // We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this... 01044 // 01045 // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are: 01046 // 0: use I2C_SLV0 01047 // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) 01048 // AK09916_REG_RSV2: we start reading here (0x03). Secret sauce... 01049 // 10: we read 10 bytes each cycle 01050 // true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) 01051 // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate 01052 // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) 01053 // true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm) 01054 // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm) 01055 result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result; 01056 // 01057 // We also need to set up I2C_SLV1 to do the Single Measurement triggering: 01058 // 1: use I2C_SLV1 01059 // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) 01060 // AK09916_REG_CNTL2: we start writing here (0x31) 01061 // 1: not sure why, but the write does not happen if this is set to zero 01062 // false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte 01063 // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear 01064 // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) 01065 // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit 01066 // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit 01067 // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample 01068 result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result; 01069 01070 // Set the I2C Master ODR configuration 01071 // It is not clear why we need to do this... But it appears to be essential! From the datasheet: 01072 // "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled. 01073 // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) 01074 // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR. 01075 // If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR." 01076 // Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)? 01077 // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case. 01078 result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 01079 uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz 01080 result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register 01081 01082 // Configure clock source through PWR_MGMT_1 01083 // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator 01084 result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails 01085 01086 // Enable accel and gyro sensors through PWR_MGMT_2 01087 // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2 01088 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 01089 uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) 01090 result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register 01091 01092 // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG 01093 // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode 01094 result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; 01095 01096 // Disable the FIFO 01097 result = enableFIFO(false); if (result > worstResult) worstResult = result; 01098 01099 // Disable the DMP 01100 result = enableDMP(false); if (result > worstResult) worstResult = result; 01101 01102 // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1 01103 // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG 01104 ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors 01105 myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) 01106 // gpm2 01107 // gpm4 01108 // gpm8 01109 // gpm16 01110 myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) 01111 // dps250 01112 // dps500 01113 // dps1000 01114 // dps2000 01115 result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result; 01116 01117 // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB)) 01118 // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte... 01119 // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway 01120 result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result; 01121 01122 // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 01123 // If we see this interrupt, we'll need to reset the FIFO 01124 //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs 01125 01126 // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2 01127 // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1 01128 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 01129 uint8_t zero = 0; 01130 result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result; 01131 // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 01132 result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result; 01133 01134 // Turn off data ready interrupt through INT_ENABLE_1 01135 result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; 01136 01137 // Reset FIFO through FIFO_RST 01138 result = resetFIFO(); if (result > worstResult) worstResult = result; 01139 01140 // Set gyro sample rate divider with GYRO_SMPLRT_DIV 01141 // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2 01142 ICM_20948_smplrt_t mySmplrt; 01143 mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). 01144 mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). 01145 //mySmplrt.g = 4; // 225Hz 01146 //mySmplrt.a = 4; // 225Hz 01147 //mySmplrt.g = 8; // 112Hz 01148 //mySmplrt.a = 8; // 112Hz 01149 result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; 01150 01151 // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL 01152 result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS 01153 01154 // Now load the DMP firmware 01155 result = loadDMPFirmware(); if (result > worstResult) worstResult = result; 01156 01157 // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL 01158 result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS 01159 01160 // Set the Hardware Fix Disable register to 0x48 01161 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 01162 uint8_t fix = 0x48; 01163 result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result; 01164 01165 // Set the Single FIFO Priority Select register to 0xE4 01166 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 01167 uint8_t fifoPrio = 0xE4; 01168 result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result; 01169 01170 // Configure Accel scaling to DMP 01171 // The DMP scales accel raw data internally to align 1g as 2^25 01172 // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g 01173 const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; 01174 result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register 01175 // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g 01176 const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; 01177 result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register 01178 01179 // Configure Compass mount matrix and scale to DMP 01180 // The mount matrix write to DMP register is used to align the compass axes with accel/gyro. 01181 // This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30. 01182 // Each compass axis will be converted as below: 01183 // X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02 01184 // Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12 01185 // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 01186 // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU. 01187 // 2^30 / 6.66666 = 161061273 = 0x9999999 01188 const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; 01189 const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example 01190 const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example 01191 result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result; 01192 result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01193 result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01194 result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01195 result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; 01196 result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01197 result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01198 result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01199 result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; 01200 01201 // Configure the B2S Mounting Matrix 01202 const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; 01203 const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example 01204 result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; 01205 result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01206 result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01207 result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01208 result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; 01209 result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01210 result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01211 result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; 01212 result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; 01213 01214 // Configure the DMP Gyro Scaling Factor 01215 // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where 01216 // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ... 01217 // 10=102.2727Hz sample rate, ... etc. 01218 // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps 01219 result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) 01220 01221 // Configure the Gyro full scale 01222 // 2000dps : 2^28 01223 // 1000dps : 2^27 01224 // 500dps : 2^26 01225 // 250dps : 2^25 01226 const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 01227 result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result; 01228 01229 // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) 01230 const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz 01231 //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz 01232 //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz 01233 result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result; 01234 01235 // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) 01236 const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz 01237 //const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz 01238 //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz 01239 result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result; 01240 01241 // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) 01242 const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz 01243 //const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz 01244 //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz 01245 result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result; 01246 01247 // Configure the Accel Cal Rate 01248 const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example 01249 result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result; 01250 01251 // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz. 01252 // Let's set the Compass Time Buffer to 69 (Hz). 01253 const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz 01254 result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result; 01255 01256 // Enable DMP interrupt 01257 // This would be the most efficient way of getting the DMP data, instead of polling the FIFO 01258 //result = intEnableDMP(true); if (result > worstResult) worstResult = result; 01259 01260 #endif 01261 01262 return worstResult; 01263 } 01264 01265 ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal) 01266 { 01267 ICM_20948_Status_e retval = ICM_20948_Stat_Ok; 01268 01269 i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL 01270 i2cMasterEnable(true); 01271 01272 resetMag(); 01273 01274 //After a ICM reset the Mag sensor may stop responding over the I2C master 01275 //Reset the Master I2C until it responds 01276 uint8_t tries = 0; 01277 while (tries < MAX_MAGNETOMETER_STARTS) 01278 { 01279 tries++; 01280 01281 //See if we can read the WhoIAm register correctly 01282 retval = magWhoIAm(); 01283 if (retval == ICM_20948_Stat_Ok) 01284 break; //WIA matched! 01285 01286 i2cMasterReset(); //Otherwise, reset the master I2C and try again 01287 01288 wait_ms(10); 01289 } 01290 01291 if (tries == MAX_MAGNETOMETER_STARTS) 01292 { 01293 if (_printDebug) { 01294 _debugSerial->printf("ICM_20948::startupMagnetometer: reached MAX_MAGNETOMETER_STARTS (%d). Returning ICM_20948_Stat_WrongID\n\r", (int)MAX_MAGNETOMETER_STARTS); 01295 } 01296 status = ICM_20948_Stat_WrongID; 01297 return status; 01298 } 01299 else 01300 { 01301 if (_printDebug) { 01302 _debugSerial->printf("ICM_20948::startupMagnetometer: successful magWhoIAm after %d", (int)tries); 01303 if (tries == 1) { 01304 _debugSerial->printf(" try\n\r"); 01305 } else { 01306 _debugSerial->printf(" tries\n\r"); 01307 } 01308 } 01309 } 01310 01311 //Return now if minimal is true. The mag will be configured manually for the DMP 01312 if (minimal) // Return now if minimal is true 01313 { 01314 if (_printDebug) { 01315 _debugSerial->printf("ICM_20948::startupMagnetometer: minimal startup complete!\n\r"); 01316 } 01317 return status; 01318 } 01319 01320 //Set up magnetometer 01321 AK09916_CNTL2_Reg_t reg; 01322 reg.MODE = AK09916_mode_cont_100hz; 01323 reg.reserved_0 = 0; // Make sure the unused bits are clear. Probably redundant, but prevents confusion when looking at the I2C traffic 01324 retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)®); 01325 if (retval != ICM_20948_Stat_Ok) 01326 { 01327 if (_printDebug) { 01328 _debugSerial->printf("ICM_20948::startupMagnetometer: writeMag returned: "); 01329 debugPrintStatus(retval); 01330 } 01331 status = retval; 01332 return status; 01333 } 01334 01335 retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false); 01336 if (retval != ICM_20948_Stat_Ok) 01337 { 01338 if (_printDebug) { 01339 _debugSerial->printf("ICM_20948::startupMagnetometer: i2cMasterConfigurePeripheral returned: "); 01340 debugPrintStatus(retval); 01341 } 01342 status = retval; 01343 return status; 01344 } 01345 01346 return status; 01347 } 01348 01349 ICM_20948_Status_e ICM_20948::magWhoIAm(void) 01350 { 01351 ICM_20948_Status_e retval = ICM_20948_Stat_Ok; 01352 01353 uint8_t whoiam1, whoiam2; 01354 whoiam1 = readMag(AK09916_REG_WIA1); 01355 // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r 01356 // i2cMasterSingleR updates status so it is OK to set retval to status here 01357 retval = status; 01358 if (retval != ICM_20948_Stat_Ok) 01359 { 01360 if (_printDebug) { 01361 _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d (should be 72) readMag set status to: ", (int)whoiam1); 01362 debugPrintStatus(status); 01363 } 01364 return retval; 01365 } 01366 whoiam2 = readMag(AK09916_REG_WIA2); 01367 // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r 01368 // i2cMasterSingleR updates status so it is OK to set retval to status here 01369 retval = status; 01370 if (retval != ICM_20948_Stat_Ok) 01371 { 01372 if (_printDebug) { 01373 _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); 01374 _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); 01375 _debugSerial->printf(" (should be 9) readMag set status to: "); 01376 debugPrintStatus(status); 01377 } 01378 return retval; 01379 } 01380 01381 if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF))) 01382 { 01383 retval = ICM_20948_Stat_Ok; 01384 status = retval; 01385 return status; 01386 } 01387 if (_printDebug) { 01388 _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); 01389 _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); 01390 _debugSerial->printf(" (should be 9). Returning ICM_20948_Stat_WrongID\n\r"); 01391 } 01392 retval = ICM_20948_Stat_WrongID; 01393 status = retval; 01394 return status; 01395 } 01396 01397 // SPI 01398 01399 // SPISettings ICM_20948_SPI_DEFAULT_SETTINGS(ICM_20948_SPI_DEFAULT_FREQ, ICM_20948_SPI_DEFAULT_ORDER, ICM_20948_SPI_DEFAULT_MODE); 01400 01401 ICM_20948_SPI::ICM_20948_SPI() 01402 { 01403 } 01404 01405 //ICM_20948_Status_e begin(DigitalOut &cspin, SPI &spiPort, uint32_t SPIFreq = ICM_20948_SPI_DEFAULT_FREQ); // TODO: check this! 01406 01407 ICM_20948_Status_e ICM_20948_SPI::begin(DigitalOut &csPin, SPI &spiPort, uint32_t SPIFreq) 01408 { 01409 if (SPIFreq > 7000000) 01410 SPIFreq = 7000000; // Limit SPI frequency to 7MHz 01411 01412 // Associate 01413 _spi = &spiPort; 01414 _spi->frequency(SPIFreq); // TODO: could also set mode here? 01415 _cs = &csPin; 01416 01417 // Set pins to default positions 01418 _cs->write(1); 01419 01420 // 'Kickstart' the SPI hardware. 01421 _spi->write(0x00); 01422 01423 // Set up the serif 01424 _serif.write = ICM_20948_write_SPI; 01425 _serif.read = ICM_20948_read_SPI; 01426 _serif.user = (void *)this; // refer to yourself in the user field 01427 01428 // Link the serif 01429 _device._serif = &_serif; 01430 01431 //#if defined(ICM_20948_USE_DMP) 01432 // _device._dmp_firmware_available = true; // Initialize _dmp_firmware_available 01433 //#else 01434 _device._dmp_firmware_available = false; // Initialize _dmp_firmware_available 01435 //#endif 01436 01437 _device._firmware_loaded = false; // Initialize _firmware_loaded 01438 _device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank. 01439 _device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems. 01440 _device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf 01441 _device._gyroSFpll = 0; 01442 _device._enabled_Android_0 = 0; // Keep track of which Android sensors are enabled: 0-31 01443 _device._enabled_Android_1 = 0; // Keep track of which Android sensors are enabled: 32- 01444 _device._enabled_Android_intr_0 = 0; // Keep track of which Android sensor interrupts are enabled: 0-31 01445 _device._enabled_Android_intr_1 = 0; // Keep track of which Android sensor interrupts are enabled: 32- 01446 01447 // Perform default startup 01448 // Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required. 01449 status = startupDefault(_device._dmp_firmware_available); 01450 if (status != ICM_20948_Stat_Ok) 01451 { 01452 if (_printDebug) { 01453 _debugSerial->printf("ICM_20948_SPI::begin: startupDefault returned: "); 01454 debugPrintStatus(status); 01455 } 01456 } 01457 01458 return status; 01459 } 01460 01461 ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *data, uint32_t len, void *user) 01462 { 01463 if (user == NULL) 01464 { 01465 return ICM_20948_Stat_ParamErr; 01466 } 01467 01468 // TODO: check this? 01469 SPI *_spi = ((ICM_20948_SPI *)user)->_spi; // Cast user field to ICM_20948_SPI type and extract the SPI interface pointer 01470 DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; 01471 if (_spi == NULL) 01472 { 01473 return ICM_20948_Stat_ParamErr; 01474 } 01475 01476 // 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states even when sharing the SPI bus with devices that use other modes 01477 _spi->write(0x00); 01478 01479 _cs->write(0); 01480 // delayMicroseconds(5); 01481 _spi->write(((reg & 0x7F) | 0x00)); 01482 // SPI.transfer(data, len); // Can't do this thanks to Arduino's poor implementation 01483 for (uint32_t indi = 0; indi < len; indi++) 01484 { 01485 _spi->write(*(data + indi)); 01486 } 01487 // delayMicroseconds(5); 01488 _cs->write(1); 01489 01490 return ICM_20948_Stat_Ok; 01491 } 01492 01493 ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user) 01494 { 01495 if (user == NULL) 01496 { 01497 return ICM_20948_Stat_ParamErr; 01498 } 01499 // TODO: also check this 01500 SPI *_spi = ((ICM_20948_SPI *)user)->_spi; 01501 DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; 01502 if (_spi == NULL) 01503 { 01504 return ICM_20948_Stat_ParamErr; 01505 } 01506 01507 // 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states 01508 _spi->write(0x00); 01509 01510 _cs->write(0); 01511 // delayMicroseconds(5); 01512 _spi->write(((reg & 0x7F) | 0x80)); 01513 // SPI.transfer(data, len); // Can't do this thanks to Arduino's stupid implementation 01514 for (uint32_t indi = 0; indi < len; indi++) 01515 { 01516 *(buff + indi) = _spi->write(0x00); 01517 } 01518 // delayMicroseconds(5); 01519 _cs->write(1); 01520 01521 return ICM_20948_Stat_Ok; 01522 }
Generated on Thu Aug 18 2022 09:06:28 by
1.7.2