Updates after reading the InvenSense App Note

Author: PaulZC

examples/Arduino/Example5_DMP/Example5_DMP.ino

@@ -6,6 +6,16 @@
  * Based on original code by:
  * Owen Lyke @ SparkFun Electronics
  * Original Creation Date: April 17 2019
+ * 
+ * ** This example is based on the InvenSense Application Note "Programming Sequence for DMP Hardware Functions".
+ * ** We are grateful to InvenSense for providing this.
+ * 
+ * ** Important note: by default the DMP functionality is disabled in the library. This is to save program memory.
+ * ** The DMP firmware takes up 14290 Bytes of program memory. To use the DMP, you will need to:
+ * ** Edit ICM_20948_C.h
+ * ** Uncomment line 29: #define ICM_20948_USE_DMP
+ * ** Save changes
+ * ** If you are using Windows, you can find ICM_20948_C.h in Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
  *
  * Please see License.md for the license information.
  *
@@ -59,6 +69,8 @@ void setup() {
   bool initialized = false;
   while( !initialized ){
 
+    // Initialize the ICM-20948
+    // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
 #ifdef USE_SPI
     myICM.begin( CS_PIN, SPI_PORT );
 #else
@@ -77,35 +89,121 @@ void setup() {
 
   SERIAL_PORT.println("Device connected!");
 
-  myICM.enableDMP(); // Enable the DMP
+  // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
 
-  if( myICM.status == ICM_20948_Stat_Ok )
-    SERIAL_PORT.println("DMP enabled!");
-  else
-  {
-    SERIAL_PORT.print("Enable DMP failed! Status is: ");
-    SERIAL_PORT.println( myICM.statusString() );
-  }
-  
-  myICM.enableFIFO(); // Enable the FIFO
+  bool success = true; // Use success to show if the configuration was successful
 
-  if( myICM.status == ICM_20948_Stat_Ok )
-    SERIAL_PORT.println("FIFO enabled!");
-  else
-  {
-    SERIAL_PORT.print("Enable FIFO failed! Status is: ");
-    SERIAL_PORT.println( myICM.statusString() );
-  }
+  // Configure clock source through PWR_MGMT_1
+  // Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
+  success &= (myICM.setClockSource(ICM_20948_Clock_Auto) == ICM_20948_Stat_Ok); // 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
+  uint8_t zero = 0;
+  success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &zero, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register
+
+  // Configure Gyro/Accel in Low power mode with LP_CONFIG
+  success &= (myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
+
+  // 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
+  success &= (myICM.setFullScale( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS ) == ICM_20948_Stat_Ok);
+
+  // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
+  //success &= (myICM.intEnableOverflowFIFO( 0x1F ) == ICM_20948_Stat_Ok); // Enable the interrupt on all FIFOs
+
+  // Turn off what goes into the FIFO through FIFO_EN, FIFO_EN_2
+  // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
+  success &= (myICM.write(AGB0_REG_FIFO_EN_1, &zero, 1) == ICM_20948_Stat_Ok);
+  // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
+  success &= (myICM.write(AGB0_REG_FIFO_EN_2, &zero, 1) == ICM_20948_Stat_Ok);
+
+  // Turn off data ready interrupt through INT_ENABLE_1
+  success &= (myICM.intEnableRawDataReady(false) == ICM_20948_Stat_Ok);
+
+  // Reset FIFO through FIFO_RST
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // 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 = 43; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 43 = 25Hz
+  mySmplrt.a = 44; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 44 = 25Hz
+  myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt );
 
-  myICM.enableSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR); // Enable the Game Rotation Vector
+  // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+  success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
 
-  if( myICM.status == ICM_20948_Stat_Ok )
-    SERIAL_PORT.println("INV_ICM20948_SENSOR_ROTATION_VECTOR enabled!");
+  // Now load the DMP firmware
+  success &= (myICM.loadDMPFirmware() == ICM_20948_Stat_Ok);
+
+  // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+  success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
+
+  // 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 0x4000000 when FSR is 4g
+  const unsigned char accScale[4] = {0x40, 0x00, 0x00, 0x00};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // Write 0x4000000 to ACC_SCALE DMP register
+  // In order to output hardware unit data as configured FSR write 0x40000 when FSR is 4g
+  const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00};
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // Write 0x40000 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
+  const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
+  const unsigned char mountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00};
+  const unsigned char mountMultiplierMinus[4] = {0xC0, 0x00, 0x00, 0x00};
+  success &= (myICM.writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+
+  // Enable the FIFO
+  success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
+
+  // Reset FIFO
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // Enable the DMP
+  success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
+
+  // Reset DMP
+  success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
+
+  // Enable DMP interrupt
+  //success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
+
+  // Check success
+  if( success )
+    SERIAL_PORT.println("DMP enabled!");
   else
   {
-    SERIAL_PORT.print("INV_ICM20948_SENSOR_ROTATION_VECTOR failed! Status is: ");
+    SERIAL_PORT.print("Enable DMP failed! Status is: ");
     SERIAL_PORT.println( myICM.statusString() );
   }
+  
 }
 
 void loop()

src/ICM_20948.cpp

@@ -769,7 +769,7 @@ uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg)
     return data;
 }
 
-ICM_20948_Status_e ICM_20948::startupDefault(void)
+ICM_20948_Status_e ICM_20948::startupDefault(bool minimal)
 {
     ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
@@ -783,19 +783,6 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         return status;
     }
 
-    if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware?
-    {
-      retval = loadDMPFirmware();
-      if (retval != ICM_20948_Stat_Ok)
-      {
-          debugPrint(F("ICM_20948::startupDefault: loadDMPFirmware returned: "));
-          debugPrintStatus(retval);
-          debugPrintln(F(""));
-          status = retval;
-          return status;
-      }
-    }
-
     retval = swReset();
     if (retval != ICM_20948_Stat_Ok)
     {
@@ -827,6 +814,12 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         return status;
     }
 
+    if (minimal) // Return now if minimal is true
+    {
+        debugPrintln(F("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)
     {
@@ -872,6 +865,7 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         status = retval;
         return status;
     }
+
     retval = enableDLPF(ICM_20948_Internal_Gyr, false);
     if (retval != ICM_20948_Stat_Ok)
     {
@@ -881,6 +875,7 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         status = retval;
         return status;
     }
+
     retval = startupMagnetometer();
     if (retval != ICM_20948_Stat_Ok)
     {
@@ -891,7 +886,7 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         return status;
     }
 
-    return status; // Leave status unchanged - return whatever it was when startupDefault was called
+    return status;
 }
 
 // direct read/write
@@ -956,8 +951,12 @@ ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data)
 
 ICM_20948_Status_e ICM_20948::enableDMP(bool enable)
 {
-    status = ICM_20948_enable_DMP(&_device, enable);
-    return status;
+    if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP?
+    {
+        status = ICM_20948_enable_DMP(&_device, enable);
+        return status;
+    }
+    return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::resetDMP(void)
@@ -968,14 +967,52 @@ ICM_20948_Status_e ICM_20948::resetDMP(void)
 
 ICM_20948_Status_e ICM_20948::loadDMPFirmware(void)
 {
-    status = ICM_20948_firmware_load(&_device);
-    return status;
+    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::enableSensor(enum inv_icm20948_sensor sensor, bool enable)
 {
+  if (_device._dmp_firmware_available == true) // Should we attempt to set the start address?
+  {
     status = inv_icm20948_enable_sensor(&_device, sensor, enable == true ? 1 : 0);
     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;
 }
 
 // I2C
@@ -1027,7 +1064,8 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
     _device._firmware_loaded = false; // Initialize _firmware_loaded
 
     // Perform default startup
-    status = startupDefault();
+    // 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)
     {
         debugPrint(F("ICM_20948_I2C::begin: startupDefault returned: "));
@@ -1205,7 +1243,8 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
     _device._firmware_loaded = false; // Initialize _firmware_loaded
 
     // Perform default startup
-    status = startupDefault();
+    // 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)
     {
         debugPrint(F("ICM_20948_SPI::begin: startupDefault returned: "));

src/ICM_20948.h

@@ -151,7 +151,7 @@ class ICM_20948
     uint8_t i2cMasterSingleR(uint8_t addr, uint8_t reg);
 
     // Default Setup
-    ICM_20948_Status_e startupDefault(void);
+    ICM_20948_Status_e startupDefault(bool minimal = false); // If minimal is true, several startup steps are skipped. If ICM_20948_USE_DMP is defined, .begin will call startupDefault with minimal set to true.
 
     // direct read/write
     ICM_20948_Status_e read(uint8_t reg, uint8_t *pdata, uint32_t len);
@@ -174,7 +174,10 @@ class ICM_20948
     ICM_20948_Status_e enableDMP(bool enable = true);
     ICM_20948_Status_e resetDMP(void);
     ICM_20948_Status_e loadDMPFirmware(void);
+    ICM_20948_Status_e setDMPstartAddress(unsigned short address = DMP_START_ADDRESS);
     ICM_20948_Status_e enableSensor(enum inv_icm20948_sensor sensor, bool enable = true);
+    ICM_20948_Status_e writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data);
+    ICM_20948_Status_e readDMPmems(unsigned short reg, unsigned int length, unsigned char *data);
 };
 
 // I2C