Trying to fix the DMP magnetometer readings

Author: PaulZC

examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino

@@ -67,7 +67,7 @@ void setup()
   WIRE_PORT.setClock(400000);
 #endif
 
-  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+  myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
 
   bool initialized = false;
   while (!initialized)
@@ -101,6 +101,41 @@ 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
+  // 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 definately 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 should also set up I2C_SLV1 to do the Single Measurement triggering:
+  // 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
+  // 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);
+
   // 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
@@ -258,8 +293,9 @@ void setup()
   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.
+  // 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
   success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]) == ICM_20948_Stat_Ok);
 
   // Enable DMP interrupt

keywords.txt

@@ -79,6 +79,7 @@ startupMagnetometer	KEYWORD2
 magWhoIAm	KEYWORD2
 readMag	KEYWORD2
 writeMag	KEYWORD2
+resetMag	KEYWORD2
 enableFIFO	KEYWORD2
 resetFIFO	KEYWORD2
 setFIFOmode	KEYWORD2

src/ICM_20948.cpp

@@ -767,17 +767,17 @@ ICM_20948_Status_e ICM_20948::i2cMasterReset()
   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)
+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);
+  status = ICM_20948_i2c_controller_configure_peripheral(&_device, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut);
   return status;
 }
 
 //Provided for backward-compatibility only. Please update to i2cControllerConfigurePeripheral and i2cControllerPeriph4Transaction.
 //https://www.oshwa.org/2020/06/29/a-resolution-to-redefine-spi-pin-names/
 ICM_20948_Status_e ICM_20948::i2cMasterConfigureSlave(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap)
 {
-  return (i2cControllerConfigurePeripheral(peripheral, addr, reg, len, Rw, enable, data_only, grp, swap));
+  return (i2cControllerConfigurePeripheral(peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, 0));
 }
 
 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)
@@ -856,7 +856,7 @@ ICM_20948_Status_e ICM_20948::startupDefault(bool minimal)
     return status;
   }
 
-  retval = startupMagnetometer();
+  retval = startupMagnetometer(minimal); // Pass the minimal startup flag to startupMagnetometer
   if (retval != ICM_20948_Stat_Ok)
   {
     debugPrint(F("ICM_20948::startupDefault: startupMagnetometer returned: "));
@@ -956,6 +956,17 @@ ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *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)
@@ -1185,13 +1196,15 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
   return status;
 }
 
-ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
+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;
@@ -1230,6 +1243,7 @@ ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
   //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 *)&reg);
   if (retval != ICM_20948_Stat_Ok)
   {
@@ -1240,6 +1254,13 @@ ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
     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/ICM_20948.h

@@ -138,7 +138,7 @@ class ICM_20948
   ICM_20948_Status_e i2cMasterReset();
 
   //Used for configuring peripherals 0-3
-  ICM_20948_Status_e i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false);
+  ICM_20948_Status_e i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false, uint8_t dataOut = 0);
   ICM_20948_Status_e i2cControllerPeriph4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr = true);
 
   //Provided for backward-compatibility only. Please update to i2cControllerConfigurePeripheral and i2cControllerPeriph4Transaction.
@@ -158,10 +158,11 @@ class ICM_20948
   ICM_20948_Status_e write(uint8_t reg, uint8_t *pdata, uint32_t len);
 
   //Mag specific
-  ICM_20948_Status_e startupMagnetometer(void);
+  ICM_20948_Status_e startupMagnetometer(bool minimal = false); // If minimal is true, several startup steps are skipped. The mag then needs to be set up manually for the DMP.
   ICM_20948_Status_e magWhoIAm(void);
   uint8_t readMag(AK09916_Reg_Addr_e reg);
   ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata);
+  ICM_20948_Status_e resetMag();
 
   //FIFO
   ICM_20948_Status_e enableFIFO(bool enable = true);

src/util/AK09916_REGISTERS.h

@@ -7,7 +7,9 @@ typedef enum
 {
   AK09916_REG_WIA1 = 0x00,
   AK09916_REG_WIA2,
-  // discontinuity
+  AK09916_REG_RSV1,
+  AK09916_REG_RSV2, // Reserved register. We start reading here when using the DMP. Secret sauce...
+  // discontinuity - containing another nine reserved registers? Secret sauce...
   AK09916_REG_ST1 = 0x10,
   AK09916_REG_HXL,
   AK09916_REG_HXH,