Modified Arduino library for ICM_20948 IMU for Nucleo boards
ICM_20948.cpp
- Committer:
- saloutos
- Date:
- 2022-01-31
- Revision:
- 0:894b603d32ee
File content as of revision 0:894b603d32ee:
#include "ICM_20948.h" #include "util/ICM_20948_REGISTERS.h" #include "util/AK09916_REGISTERS.h" // Forward Declarations ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); // Base ICM_20948::ICM_20948() { status = ICM_20948_init_struct(&_device); } void ICM_20948::enableDebugging(Serial &debugPort) { _debugSerial = &debugPort; //Grab which port the user wants us to use for debugging _printDebug = true; //Should we print the commands we send? Good for debugging } void ICM_20948::disableDebugging(void) { _printDebug = false; //Turn off extra print statements } void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat) { // if (_printDebug == true) { switch (stat) { case ICM_20948_Stat_Ok: _debugSerial->printf("All is well.\n\r"); break; case ICM_20948_Stat_Err: _debugSerial->printf("General Error\n\r"); break; case ICM_20948_Stat_NotImpl: _debugSerial->printf("Not Implemented\n\r"); break; case ICM_20948_Stat_ParamErr: _debugSerial->printf("Parameter Error\n\r"); break; case ICM_20948_Stat_WrongID: _debugSerial->printf("Wrong ID\n\r"); break; case ICM_20948_Stat_InvalSensor: _debugSerial->printf("Invalid Sensor\n\r"); break; case ICM_20948_Stat_NoData: _debugSerial->printf("Data Underflow\n\r"); break; case ICM_20948_Stat_SensorNotSupported: _debugSerial->printf("Sensor Not Supported\n\r"); break; case ICM_20948_Stat_DMPNotSupported: _debugSerial->printf("DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?\n\r"); break; case ICM_20948_Stat_DMPVerifyFail: _debugSerial->printf("DMP Firmware Verification Failed\n\r"); break; case ICM_20948_Stat_FIFONoDataAvail: _debugSerial->printf("No FIFO Data Available\n\r"); break; case ICM_20948_Stat_FIFOIncompleteData: _debugSerial->printf("DMP data in FIFO was incomplete\n\r"); break; case ICM_20948_Stat_FIFOMoreDataAvail: _debugSerial->printf("More FIFO Data Available\n\r"); break; case ICM_20948_Stat_UnrecognisedDMPHeader: _debugSerial->printf("Unrecognised DMP Header\n\r"); break; case ICM_20948_Stat_UnrecognisedDMPHeader2: _debugSerial->printf("Unrecognised DMP Header2\n\r"); break; case ICM_20948_Stat_InvalDMPRegister: _debugSerial->printf("Invalid DMP Register\n\r"); break; default: _debugSerial->printf("Unknown Status\n\r"); break; } // } // end if debugging==true } ICM_20948_AGMT_t ICM_20948::getAGMT(void) { status = ICM_20948_get_agmt(&_device, &agmt); return agmt; } float ICM_20948::magX(void) { return getMagUT(agmt.mag.axes.x); } float ICM_20948::magY(void) { return getMagUT(agmt.mag.axes.y); } float ICM_20948::magZ(void) { return getMagUT(agmt.mag.axes.z); } float ICM_20948::getMagUT(int16_t axis_val) { return (((float)axis_val) * 0.15); } float ICM_20948::accX(void) { return getAccMG(agmt.acc.axes.x); } float ICM_20948::accY(void) { return getAccMG(agmt.acc.axes.y); } float ICM_20948::accZ(void) { return getAccMG(agmt.acc.axes.z); } float ICM_20948::getAccMG(int16_t axis_val) { switch (agmt.fss.a) { case 0: return (((float)axis_val) / 16.384); break; case 1: return (((float)axis_val) / 8.192); break; case 2: return (((float)axis_val) / 4.096); break; case 3: return (((float)axis_val) / 2.048); break; default: return 0; break; } } float ICM_20948::gyrX(void) { return getGyrDPS(agmt.gyr.axes.x); } float ICM_20948::gyrY(void) { return getGyrDPS(agmt.gyr.axes.y); } float ICM_20948::gyrZ(void) { return getGyrDPS(agmt.gyr.axes.z); } float ICM_20948::getGyrDPS(int16_t axis_val) { switch (agmt.fss.g) { case 0: return (((float)axis_val) / 131); break; case 1: return (((float)axis_val) / 65.5); break; case 2: return (((float)axis_val) / 32.8); break; case 3: return (((float)axis_val) / 16.4); break; default: return 0; break; } } float ICM_20948::temp(void) { return getTempC(agmt.tmp.val); } float ICM_20948::getTempC(int16_t val) { return (((float)val - 21) / 333.87) + 21; } // Device Level ICM_20948_Status_e ICM_20948::setBank(uint8_t bank) { status = ICM_20948_set_bank(&_device, bank); return status; } ICM_20948_Status_e ICM_20948::swReset(void) { status = ICM_20948_sw_reset(&_device); return status; } ICM_20948_Status_e ICM_20948::sleep(bool on) { status = ICM_20948_sleep(&_device, on); return status; } ICM_20948_Status_e ICM_20948::lowPower(bool on) { status = ICM_20948_low_power(&_device, on); return status; } ICM_20948_Status_e ICM_20948::setClockSource(ICM_20948_PWR_MGMT_1_CLKSEL_e source) { status = ICM_20948_set_clock_source(&_device, source); return status; } ICM_20948_Status_e ICM_20948::checkID(void) { status = ICM_20948_check_id(&_device); if (status != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::checkID: ICM_20948_check_id returned: "); debugPrintStatus(status); } } return status; } bool ICM_20948::dataReady(void) { status = ICM_20948_data_ready(&_device); if (status == ICM_20948_Stat_Ok) { return true; } return false; } uint8_t ICM_20948::getWhoAmI(void) { uint8_t retval = 0x00; status = ICM_20948_get_who_am_i(&_device, &retval); return retval; } bool ICM_20948::isConnected(void) { status = checkID(); if (status == ICM_20948_Stat_Ok) { return true; } if (_printDebug) { _debugSerial->printf("ICM_20948::isConnected: checkID returned: "); debugPrintStatus(status); } return false; } // Internal Sensor Options ICM_20948_Status_e ICM_20948::setSampleMode(uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode) { status = ICM_20948_set_sample_mode(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode); wait_ms(1); // Give the ICM20948 time to change the sample mode (see issue #8) return status; } ICM_20948_Status_e ICM_20948::setFullScale(uint8_t sensor_id_bm, ICM_20948_fss_t fss) { status = ICM_20948_set_full_scale(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, fss); return status; } ICM_20948_Status_e ICM_20948::setDLPFcfg(uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg) { status = ICM_20948_set_dlpf_cfg(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, cfg); return status; } ICM_20948_Status_e ICM_20948::enableDLPF(uint8_t sensor_id_bm, bool enable) { status = ICM_20948_enable_dlpf(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, enable); return status; } ICM_20948_Status_e ICM_20948::setSampleRate(uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt) { status = ICM_20948_set_sample_rate(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, smplrt); return status; } // Interrupts on INT Pin ICM_20948_Status_e ICM_20948::clearInterrupts(void) { ICM_20948_INT_STATUS_t int_stat; ICM_20948_INT_STATUS_1_t int_stat_1; // read to clear interrupts status = ICM_20948_set_bank(&_device, 0); if (status != ICM_20948_Stat_Ok) { return status; } status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t)); if (status != ICM_20948_Stat_Ok) { return status; } status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS_1, (uint8_t *)&int_stat_1, sizeof(ICM_20948_INT_STATUS_1_t)); if (status != ICM_20948_Stat_Ok) { return status; } // todo: there may be additional interrupts that need to be cleared, like FIFO overflow/watermark return status; } ICM_20948_Status_e ICM_20948::cfgIntActiveLow(bool active_low) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.INT1_ACTL = active_low; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::cfgIntOpenDrain(bool open_drain) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.INT1_OPEN = open_drain; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::cfgIntLatch(bool latching) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.INT1_LATCH_EN = latching; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::cfgIntAnyReadToClear(bool enabled) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.INT_ANYRD_2CLEAR = enabled; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::cfgFsyncActiveLow(bool active_low) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.ACTL_FSYNC = active_low; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::cfgFsyncIntMode(bool interrupt_mode) { ICM_20948_INT_PIN_CFG_t reg; status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase if (status != ICM_20948_Stat_Ok) { return status; } reg.FSYNC_INT_MODE_EN = interrupt_mode; // set the setting status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase if (status != ICM_20948_Stat_Ok) { return status; } return status; } // All these individual functions will use a read->set->write method to leave other settings untouched ICM_20948_Status_e ICM_20948::intEnableI2C(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.I2C_MST_INT_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.I2C_MST_INT_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableDMP(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.DMP_INT1_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.DMP_INT1_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnablePLL(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.PLL_RDY_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.PLL_RDY_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableWOM(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.WOM_INT_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.WOM_INT_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableWOF(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.REG_WOF_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.REG_WOF_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.RAW_DATA_0_RDY_EN = enable; // change the setting status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (en.RAW_DATA_0_RDY_EN != enable) { status = ICM_20948_Stat_Err; return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableOverflowFIFO(uint8_t bm_enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.FIFO_OVERFLOW_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings en.FIFO_OVERFLOW_EN_1 = ((bm_enable >> 1) & 0x01); en.FIFO_OVERFLOW_EN_2 = ((bm_enable >> 2) & 0x01); en.FIFO_OVERFLOW_EN_3 = ((bm_enable >> 3) & 0x01); en.FIFO_OVERFLOW_EN_4 = ((bm_enable >> 4) & 0x01); status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::intEnableWatermarkFIFO(uint8_t bm_enable) { ICM_20948_INT_enable_t en; // storage status = ICM_20948_int_enable(&_device, NULL, &en); // read phase if (status != ICM_20948_Stat_Ok) { return status; } en.FIFO_WM_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings en.FIFO_WM_EN_1 = ((bm_enable >> 1) & 0x01); en.FIFO_WM_EN_2 = ((bm_enable >> 2) & 0x01); en.FIFO_WM_EN_3 = ((bm_enable >> 3) & 0x01); en.FIFO_WM_EN_4 = ((bm_enable >> 4) & 0x01); status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } return status; } ICM_20948_Status_e ICM_20948::WOMThreshold(uint8_t threshold) { ICM_20948_ACCEL_WOM_THR_t thr; // storage status = ICM_20948_wom_threshold(&_device, NULL, &thr); // read phase if (status != ICM_20948_Stat_Ok) { return status; } thr.WOM_THRESHOLD = threshold; // change the setting status = ICM_20948_wom_threshold(&_device, &thr, &thr); // write phase w/ readback if (status != ICM_20948_Stat_Ok) { return status; } if (thr.WOM_THRESHOLD != threshold) { status = ICM_20948_Stat_Err; return status; } return status; } // Interface Options ICM_20948_Status_e ICM_20948::i2cMasterPassthrough(bool passthrough) { status = ICM_20948_i2c_master_passthrough(&_device, passthrough); return status; } ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable) { status = ICM_20948_i2c_master_enable(&_device, enable); return status; } ICM_20948_Status_e ICM_20948::i2cMasterReset() { status = ICM_20948_i2c_master_reset(&_device); return status; } 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) { status = ICM_20948_i2c_controller_configure_peripheral(&_device, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut); return status; } 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) { status = ICM_20948_i2c_controller_periph4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr); return status; } ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data) { status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data); return status; } uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg) { uint8_t data = 0; status = ICM_20948_i2c_master_single_r(&_device, addr, reg, &data); if (status != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::i2cMasterSingleR: ICM_20948_i2c_master_single_r returned: "); debugPrintStatus(status); } } return data; } ICM_20948_Status_e ICM_20948::startupDefault(bool minimal) { ICM_20948_Status_e retval = ICM_20948_Stat_Ok; retval = checkID(); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: checkID returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = swReset(); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: swReset returned: "); debugPrintStatus(retval); } status = retval; return status; } wait_ms(50); retval = sleep(false); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: sleep returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = lowPower(false); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: lowPower returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = startupMagnetometer(minimal); // Pass the minimal startup flag to startupMagnetometer if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: startupMagnetometer returned: "); debugPrintStatus(retval); } status = retval; return status; } if (minimal) // Return now if minimal is true { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: minimal startup complete!"); } return status; } 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 if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: setSampleMode returned: "); debugPrintStatus(retval); } status = retval; return status; } // sensors: ICM_20948_Internal_Acc, ICM_20948_Internal_Gyr, ICM_20948_Internal_Mst ICM_20948_fss_t FSS; FSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) FSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) retval = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: setFullScale returned: "); debugPrintStatus(retval); } status = retval; return status; } ICM_20948_dlpcfg_t dlpcfg; dlpcfg.a = acc_d473bw_n499bw; dlpcfg.g = gyr_d361bw4_n376bw5; retval = setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: setDLPFcfg returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = enableDLPF(ICM_20948_Internal_Acc, false); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Acc) returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = enableDLPF(ICM_20948_Internal_Gyr, false); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Gyr) returned: "); debugPrintStatus(retval); } status = retval; return status; } return status; } // direct read/write ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len) { status = ICM_20948_execute_r(&_device, reg, pdata, len); return (status); } ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len) { status = ICM_20948_execute_w(&_device, reg, pdata, len); return (status); } uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg) { uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg); // i2cMasterSingleR updates status too return data; } ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata) { status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, *pdata); return status; } ICM_20948_Status_e ICM_20948::resetMag() { uint8_t SRST = 1; // SRST: Soft reset // “0”: Normal // “1”: Reset // When “1” is set, all registers are initialized. After reset, SRST bit turns to “0” automatically. status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL3, SRST); return status; } // FIFO ICM_20948_Status_e ICM_20948::enableFIFO(bool enable) { status = ICM_20948_enable_FIFO(&_device, enable); return status; } ICM_20948_Status_e ICM_20948::resetFIFO(void) { status = ICM_20948_reset_FIFO(&_device); return status; } ICM_20948_Status_e ICM_20948::setFIFOmode(bool snapshot) { // Default to Stream (non-Snapshot) mode status = ICM_20948_set_FIFO_mode(&_device, snapshot); return status; } ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count) { status = ICM_20948_get_FIFO_count(&_device, count); return status; } ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data, uint8_t len) { status = ICM_20948_read_FIFO(&_device, data, len); return status; } // DMP ICM_20948_Status_e ICM_20948::enableDMP(bool enable) { if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP? { status = ICM_20948_enable_DMP(&_device, enable == true ? 1 : 0); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::resetDMP(void) { status = ICM_20948_reset_DMP(&_device); return status; } ICM_20948_Status_e ICM_20948::loadDMPFirmware(void) { if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware? { status = ICM_20948_firmware_load(&_device); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::setDMPstartAddress(unsigned short address) { if (_device._dmp_firmware_available == true) // Should we attempt to set the start address? { status = ICM_20948_set_dmp_start_address(&_device, address); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable) { if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor? { status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0); if (_printDebug) { _debugSerial->printf("ICM_20948::enableDMPSensor: _enabled_Android_0: %d", (int)_device._enabled_Android_0); _debugSerial->printf(" _enabled_Android_1: %d", (int)_device._enabled_Android_1); _debugSerial->printf(" _dataOutCtl1: %d", (int)_device._dataOutCtl1); _debugSerial->printf(" _dataOutCtl2: %d", (int)_device._dataOutCtl2); _debugSerial->printf(" _dataRdyStatus: %d\n\r", (int)_device._dataRdyStatus); } return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable) { if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor interrupt? { status = inv_icm20948_enable_dmp_sensor_int(&_device, sensor, enable == true ? 1 : 0); if (_printDebug) { _debugSerial->printf("ICM_20948::enableDMPSensorInt: _enabled_Android_intr_0: %d", (int)_device._enabled_Android_intr_0); _debugSerial->printf(" _enabled_Android_intr_1: %d", (int)_device._enabled_Android_intr_1); _debugSerial->printf(" _dataIntrCtl: %d\n\r", (int)_device._dataIntrCtl); } return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data) { if (_device._dmp_firmware_available == true) // Should we attempt to write to the DMP? { status = inv_icm20948_write_mems(&_device, reg, length, data); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int length, unsigned char *data) { if (_device._dmp_firmware_available == true) // Should we attempt to read from the DMP? { status = inv_icm20948_read_mems(&_device, reg, length, data); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval) { if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR? { // 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. // Setting value can be calculated as follows: // Value = (DMP running rate (225Hz) / ODR ) - 1 // E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8. status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data) { if (_device._dmp_firmware_available == true) // Should we attempt to set the data from the FIFO? { status = inv_icm20948_read_dmp_data(&_device, data); return status; } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e ICM_20948::setGyroSF(unsigned char div, int gyro_level) { if (_device._dmp_firmware_available == true) // Should we attempt to set the Gyro SF? { status = inv_icm20948_set_gyro_sf(&_device, div, gyro_level); if (_printDebug) { _debugSerial->printf("ICM_20948::setGyroSF: pll: %d", (int)_device._gyroSFpll); _debugSerial->printf(" Gyro SF is: %d\n\r", (int)_device._gyroSF); } return status; } return ICM_20948_Stat_DMPNotSupported; } // Combine all of the DMP start-up code from the earlier DMP examples // This function is defined as __attribute__((weak)) so you can overwrite it if you want to, // e.g. to modify the sample rate ICM_20948_Status_e ICM_20948::initializeDMP(void) { // First, let's check if the DMP is available if (_device._dmp_firmware_available != true) { if (_printDebug) { _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"); } return ICM_20948_Stat_DMPNotSupported; } ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; #if defined(ICM_20948_USE_DMP) // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions". ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read // nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. // Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit. // // But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, // when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved // RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here... // The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04. // We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this... // // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are: // 0: use I2C_SLV0 // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) // AK09916_REG_RSV2: we start reading here (0x03). Secret sauce... // 10: we read 10 bytes each cycle // true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) // 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) // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm) result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result; // // We also need to set up I2C_SLV1 to do the Single Measurement triggering: // 1: use I2C_SLV1 // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) // AK09916_REG_CNTL2: we start writing here (0x31) // 1: not sure why, but the write does not happen if this is set to zero // false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result; // Set the I2C Master ODR configuration // It is not clear why we need to do this... But it appears to be essential! From the datasheet: // "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled. // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR. // If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR." // 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)? // 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. result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz 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 // Configure clock source through PWR_MGMT_1 // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails // Enable accel and gyro sensors through PWR_MGMT_2 // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG // 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 result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; // Disable the FIFO result = enableFIFO(false); if (result > worstResult) worstResult = result; // Disable the DMP result = enableDMP(false); if (result > worstResult) worstResult = result; // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1 // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) // gpm2 // gpm4 // gpm8 // gpm16 myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) // dps250 // dps500 // dps1000 // dps2000 result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result; // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB)) // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte... // 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 result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result; // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 // If we see this interrupt, we'll need to reset the FIFO //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2 // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 uint8_t zero = 0; result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result; // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result; // Turn off data ready interrupt through INT_ENABLE_1 result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; // Reset FIFO through FIFO_RST result = resetFIFO(); if (result > worstResult) worstResult = result; // Set gyro sample rate divider with GYRO_SMPLRT_DIV // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2 ICM_20948_smplrt_t mySmplrt; 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). 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). //mySmplrt.g = 4; // 225Hz //mySmplrt.a = 4; // 225Hz //mySmplrt.g = 8; // 112Hz //mySmplrt.a = 8; // 112Hz result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS // Now load the DMP firmware result = loadDMPFirmware(); if (result > worstResult) worstResult = result; // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS // Set the Hardware Fix Disable register to 0x48 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 uint8_t fix = 0x48; result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result; // Set the Single FIFO Priority Select register to 0xE4 result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 uint8_t fifoPrio = 0xE4; result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result; // Configure Accel scaling to DMP // The DMP scales accel raw data internally to align 1g as 2^25 // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register // Configure Compass mount matrix and scale to DMP // The mount matrix write to DMP register is used to align the compass axes with accel/gyro. // This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30. // Each compass axis will be converted as below: // X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02 // Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12 // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU. // 2^30 / 6.66666 = 161061273 = 0x9999999 const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; // Configure the B2S Mounting Matrix const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; // Configure the DMP Gyro Scaling Factor // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ... // 10=102.2727Hz sample rate, ... etc. // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) // Configure the Gyro full scale // 2000dps : 2^28 // 1000dps : 2^27 // 500dps : 2^26 // 250dps : 2^25 const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result; // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result; // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz //const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result; // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz //const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result; // Configure the Accel Cal Rate const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result; // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz. // Let's set the Compass Time Buffer to 69 (Hz). const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result; // Enable DMP interrupt // This would be the most efficient way of getting the DMP data, instead of polling the FIFO //result = intEnableDMP(true); if (result > worstResult) worstResult = result; #endif return worstResult; } ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal) { ICM_20948_Status_e retval = ICM_20948_Stat_Ok; i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL i2cMasterEnable(true); resetMag(); //After a ICM reset the Mag sensor may stop responding over the I2C master //Reset the Master I2C until it responds uint8_t tries = 0; while (tries < MAX_MAGNETOMETER_STARTS) { tries++; //See if we can read the WhoIAm register correctly retval = magWhoIAm(); if (retval == ICM_20948_Stat_Ok) break; //WIA matched! i2cMasterReset(); //Otherwise, reset the master I2C and try again wait_ms(10); } if (tries == MAX_MAGNETOMETER_STARTS) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupMagnetometer: reached MAX_MAGNETOMETER_STARTS (%d). Returning ICM_20948_Stat_WrongID\n\r", (int)MAX_MAGNETOMETER_STARTS); } status = ICM_20948_Stat_WrongID; return status; } else { if (_printDebug) { _debugSerial->printf("ICM_20948::startupMagnetometer: successful magWhoIAm after %d", (int)tries); if (tries == 1) { _debugSerial->printf(" try\n\r"); } else { _debugSerial->printf(" tries\n\r"); } } } //Return now if minimal is true. The mag will be configured manually for the DMP if (minimal) // Return now if minimal is true { if (_printDebug) { _debugSerial->printf("ICM_20948::startupMagnetometer: minimal startup complete!\n\r"); } return status; } //Set up magnetometer AK09916_CNTL2_Reg_t reg; reg.MODE = AK09916_mode_cont_100hz; reg.reserved_0 = 0; // Make sure the unused bits are clear. Probably redundant, but prevents confusion when looking at the I2C traffic retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)®); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupMagnetometer: writeMag returned: "); debugPrintStatus(retval); } status = retval; return status; } retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false); if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::startupMagnetometer: i2cMasterConfigurePeripheral returned: "); debugPrintStatus(retval); } status = retval; return status; } return status; } ICM_20948_Status_e ICM_20948::magWhoIAm(void) { ICM_20948_Status_e retval = ICM_20948_Stat_Ok; uint8_t whoiam1, whoiam2; whoiam1 = readMag(AK09916_REG_WIA1); // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r // i2cMasterSingleR updates status so it is OK to set retval to status here retval = status; if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d (should be 72) readMag set status to: ", (int)whoiam1); debugPrintStatus(status); } return retval; } whoiam2 = readMag(AK09916_REG_WIA2); // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r // i2cMasterSingleR updates status so it is OK to set retval to status here retval = status; if (retval != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); _debugSerial->printf(" (should be 9) readMag set status to: "); debugPrintStatus(status); } return retval; } if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF))) { retval = ICM_20948_Stat_Ok; status = retval; return status; } if (_printDebug) { _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); _debugSerial->printf(" (should be 9). Returning ICM_20948_Stat_WrongID\n\r"); } retval = ICM_20948_Stat_WrongID; status = retval; return status; } // SPI // SPISettings ICM_20948_SPI_DEFAULT_SETTINGS(ICM_20948_SPI_DEFAULT_FREQ, ICM_20948_SPI_DEFAULT_ORDER, ICM_20948_SPI_DEFAULT_MODE); ICM_20948_SPI::ICM_20948_SPI() { } //ICM_20948_Status_e begin(DigitalOut &cspin, SPI &spiPort, uint32_t SPIFreq = ICM_20948_SPI_DEFAULT_FREQ); // TODO: check this! ICM_20948_Status_e ICM_20948_SPI::begin(DigitalOut &csPin, SPI &spiPort, uint32_t SPIFreq) { if (SPIFreq > 7000000) SPIFreq = 7000000; // Limit SPI frequency to 7MHz // Associate _spi = &spiPort; _spi->frequency(SPIFreq); // TODO: could also set mode here? _cs = &csPin; // Set pins to default positions _cs->write(1); // 'Kickstart' the SPI hardware. _spi->write(0x00); // Set up the serif _serif.write = ICM_20948_write_SPI; _serif.read = ICM_20948_read_SPI; _serif.user = (void *)this; // refer to yourself in the user field // Link the serif _device._serif = &_serif; //#if defined(ICM_20948_USE_DMP) // _device._dmp_firmware_available = true; // Initialize _dmp_firmware_available //#else _device._dmp_firmware_available = false; // Initialize _dmp_firmware_available //#endif _device._firmware_loaded = false; // Initialize _firmware_loaded _device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank. _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. _device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf _device._gyroSFpll = 0; _device._enabled_Android_0 = 0; // Keep track of which Android sensors are enabled: 0-31 _device._enabled_Android_1 = 0; // Keep track of which Android sensors are enabled: 32- _device._enabled_Android_intr_0 = 0; // Keep track of which Android sensor interrupts are enabled: 0-31 _device._enabled_Android_intr_1 = 0; // Keep track of which Android sensor interrupts are enabled: 32- // Perform default startup // Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required. status = startupDefault(_device._dmp_firmware_available); if (status != ICM_20948_Stat_Ok) { if (_printDebug) { _debugSerial->printf("ICM_20948_SPI::begin: startupDefault returned: "); debugPrintStatus(status); } } return status; } ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *data, uint32_t len, void *user) { if (user == NULL) { return ICM_20948_Stat_ParamErr; } // TODO: check this? SPI *_spi = ((ICM_20948_SPI *)user)->_spi; // Cast user field to ICM_20948_SPI type and extract the SPI interface pointer DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; if (_spi == NULL) { return ICM_20948_Stat_ParamErr; } // '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 _spi->write(0x00); _cs->write(0); // delayMicroseconds(5); _spi->write(((reg & 0x7F) | 0x00)); // SPI.transfer(data, len); // Can't do this thanks to Arduino's poor implementation for (uint32_t indi = 0; indi < len; indi++) { _spi->write(*(data + indi)); } // delayMicroseconds(5); _cs->write(1); return ICM_20948_Stat_Ok; } ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user) { if (user == NULL) { return ICM_20948_Stat_ParamErr; } // TODO: also check this SPI *_spi = ((ICM_20948_SPI *)user)->_spi; DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; if (_spi == NULL) { return ICM_20948_Stat_ParamErr; } // 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states _spi->write(0x00); _cs->write(0); // delayMicroseconds(5); _spi->write(((reg & 0x7F) | 0x80)); // SPI.transfer(data, len); // Can't do this thanks to Arduino's stupid implementation for (uint32_t indi = 0; indi < len; indi++) { *(buff + indi) = _spi->write(0x00); } // delayMicroseconds(5); _cs->write(1); return ICM_20948_Stat_Ok; }