Andrew SaLoutos
Modified Arduino library for ICM_20948 IMU for Nucleo boards
Diff: ICM_20948.cpp
- Revision:
- 0:894b603d32ee
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ICM_20948.cpp Mon Jan 31 03:25:31 2022 +0000
@@ -0,0 +1,1522 @@
+#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;
+}