Adding I2C Master ODR configuration

Author: PaulZC

examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino

@@ -2,7 +2,7 @@
  * Example10_DMP_FastMultipleSensors.ino
  * ICM 20948 Arduino Library Demo
  * Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
- * Paul Clark, February 15th 2021
+ * Paul Clark, April 25th, 2021
  * Based on original code by:
  * Owen Lyke @ SparkFun Electronics
  * Original Creation Date: April 17 2019
@@ -101,6 +101,54 @@ void setup()
 
   bool success = true; // Use success 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)
+  success &= (myICM.i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true) == ICM_20948_Stat_Ok);
+  //
+  // 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
+  success &= (myICM.i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single) == ICM_20948_Stat_Ok);
+
+  // 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.
+  success &= (myICM.setBank(3) == ICM_20948_Stat_Ok); // Select Bank 3
+  uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
+  success &= (myICM.write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1) == ICM_20948_Stat_Ok); // 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
   success &= (myICM.setClockSource(ICM_20948_Clock_Auto) == ICM_20948_Stat_Ok); // This is shorthand: success will be set to false if setClockSource fails
@@ -257,9 +305,9 @@ void setup()
   const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]) == ICM_20948_Stat_Ok);
 
-  // Configure the Compass Time Buffer. The compass (magnetometer) is set to 100Hz (AK09916_mode_cont_100hz)
-  // in startupMagnetometer. We need to set CPASS_TIME_BUFFER to 100 too.
-  const unsigned char compassRate[2] = {0x00, 0x64}; // 100Hz
+  // 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
   success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]) == ICM_20948_Stat_Ok);
 
   // Enable DMP interrupt

examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino

@@ -2,7 +2,7 @@
  * Example9_DMP_MultipleSensors.ino
  * ICM 20948 Arduino Library Demo
  * Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
- * Paul Clark, February 15th 2021
+ * Paul Clark, April 25th, 2021
  * Based on original code by:
  * Owen Lyke @ SparkFun Electronics
  * Original Creation Date: April 17 2019
@@ -101,14 +101,14 @@ void setup()
 
   bool success = true; // Use success to show if the configuration was successful
 
-  // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, called by begin) configures the AK09916 magnetometer
+  // 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
+  // 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 definately some secret sauce in here...
+  // 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...
   //
@@ -124,7 +124,9 @@ void setup()
   // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm)
   success &= (myICM.i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true) == ICM_20948_Stat_Ok);
   //
-  // We should also set up I2C_SLV1 to do the Single Measurement triggering:
+  // 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
@@ -133,8 +135,19 @@ void setup()
   // 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
-  // except doing that causes the magnetometer to stall and give out stale data. So, for now, the next line needs to stay commented!
-  //success &= (myICM.i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single) == ICM_20948_Stat_Ok);
+  success &= (myICM.i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single) == ICM_20948_Stat_Ok);
+
+  // 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.
+  success &= (myICM.setBank(3) == ICM_20948_Stat_Ok); // Select Bank 3
+  uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
+  success &= (myICM.write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1) == ICM_20948_Stat_Ok); // 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
@@ -143,7 +156,7 @@ void setup()
   // Enable accel and gyro sensors through PWR_MGMT_2
   // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
   success &= (myICM.setBank(0) == ICM_20948_Stat_Ok);                               // Select Bank 0
-  uint8_t pwrMgmt2 = 0x40;                                                          // Set the reserved bit 6
+  uint8_t pwrMgmt2 = 0x40;                                                          // Set the reserved bit 6 (pressure sensor disable?)
   success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register
 
   // Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
@@ -292,10 +305,9 @@ void setup()
   const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]) == ICM_20948_Stat_Ok);
 
-  // Configure the Compass Time Buffer. The compass (magnetometer) is set to 100Hz (AK09916_mode_cont_100hz)
-  // in startupMagnetometer. I think we need to set CPASS_TIME_BUFFER to 100 too.
-  const unsigned char compassRate[2] = {0x00, 0x64}; // 100Hz
-  //const unsigned char compassRate[2] = {0x04, 0x65}; // 1125Hz - same as the accelerometer sample rate
+  // 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
   success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]) == ICM_20948_Stat_Ok);
 
   // Enable DMP interrupt
@@ -449,6 +461,22 @@ void loop()
       SERIAL_PORT.print(F(" Z:"));
       SERIAL_PORT.println(z);
     }
+
+    if ((data.header & DMP_header_bitmap_Header2) > 0) // Header 2
+    {
+      if ((data.header2 & DMP_header2_bitmap_Secondary_On_Off) > 0) // Secondary On/Off
+      {
+        if (data.Secondary_On_Off.Sensors.Gyro_Off > 0) SERIAL_PORT.println(F("Gyro Off"));
+        if (data.Secondary_On_Off.Sensors.Gyro_On > 0) SERIAL_PORT.println(F("Gyro On"));
+        if (data.Secondary_On_Off.Sensors.Compass_Off > 0) SERIAL_PORT.println(F("Compass Off"));
+        if (data.Secondary_On_Off.Sensors.Compass_On > 0) SERIAL_PORT.println(F("Compass On"));
+      }
+      if ((data.header2 & DMP_header2_bitmap_Fsync) > 0) // Fsync delay
+      {
+        SERIAL_PORT.print(F("Fsync delay: "));
+        SERIAL_PORT.println(data.Fsync_Delay_Time);
+      }
+    }
   }
 
   if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay

src/ICM_20948.cpp

@@ -1248,6 +1248,13 @@ ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal)
       debugPrintln(F(" tries"));
   }
 
+  //Return now if minimal is true. The mag will be configured manually for the DMP
+  if (minimal) // Return now if minimal is true
+  {
+    debugPrintln(F("ICM_20948::startupMagnetometer: minimal startup complete!"));
+    return status;
+  }
+
   //Set up magnetometer
   AK09916_CNTL2_Reg_t reg;
   reg.MODE = AK09916_mode_cont_100hz;
@@ -1262,13 +1269,6 @@ ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal)
     return status;
   }
 
-  //Return now if minimal is true. The mag will be configured manually for the DMP
-  if (minimal) // Return now if minimal is true
-  {
-    debugPrintln(F("ICM_20948::startupMagnetometer: minimal startup complete!"));
-    return status;
-  }
-
   retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false);
   if (retval != ICM_20948_Stat_Ok)
   {

src/util/ICM_20948_C.h

@@ -25,7 +25,7 @@ extern "C"
 extern int memcmp(const void *, const void *, size_t); // Avoid compiler warnings
 
 // Define if the DMP will be supported
-// Note: you must have 14301 Bytes of program memory available to store the DMP firmware!
+// Note: you must have 14290/14301 Bytes of program memory available to store the DMP firmware!
 #define ICM_20948_USE_DMP // Uncomment this line to enable DMP support.
 
 #define ICM_20948_I2C_ADDR_AD0 0x68 // Or 0x69 when AD0 is high