DMP Improvements

DMP functionality now seems good; Euler angles are well-behaved.
Ensured the magnetometer is started correctly.
Corrected the compass matrix values in the DMP examples.
Added the B2S matrix in the DMP examples.
Added new examples for Quat6 (with Euler angles) and RawAccel.
Improved readFIFO so it can read multiple bytes consecutively (substantially improving the I2C throughput).

Author: PaulZC

examples/Arduino/Example6_DualSPITest/Example6_DualSPITest.ino renamed to examples/Arduino/Example5_DualSPITest/Example5_DualSPITest.ino

@@ -1,5 +1,5 @@
 /****************************************************************
- * Example6_DualSPITest.ino
+ * Example5_DualSPITest.ino
  * ICM 20948 Arduino Library Demo
  * Use the default configuration to stream 9-axis IMU data on two IMUs over SPI
  * Owen Lyke @ SparkFun Electronics

examples/Arduino/Example5_DMP_Quat9_Orientation/Example5_DMP_Quat9_Orientation.ino renamed to examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino

@@ -1,5 +1,5 @@
 /****************************************************************
- * Example5_DMP_Quat9_Orientation.ino
+ * Example6_DMP_Quat9_Orientation.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
@@ -83,7 +83,7 @@ void setup() {
     SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
     SERIAL_PORT.println( myICM.statusString() );
     if( myICM.status != ICM_20948_Stat_Ok ){
-      SERIAL_PORT.println( "Trying again..." );
+      SERIAL_PORT.println( F("Trying again...") );
       delay(500);
     }else{
       initialized = true;
@@ -106,8 +106,14 @@ void setup() {
   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 (cycled) with LP_CONFIG
-  success &= (myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
+  // Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
+  success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
+
+  // Disable the FIFO
+  success &= (myICM.enableFIFO(false) == ICM_20948_Stat_Ok);
+
+  // Disable the DMP
+  success &= (myICM.enableDMP(false) == 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
@@ -172,11 +178,9 @@ void setup() {
   // 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
-  // Magnetometer full scale is +/- 4900uT so _I think_ we need to multiply by 2^30 / 4900 = 0x000357FA
-  // The magnetometer Y and Z axes are reversed compared to the accelerometer so we'll invert those
   const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
-  const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
-  const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
+  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
   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);
@@ -187,22 +191,27 @@ void setup() {
   success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
   success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
 
-//  // Configure the biases
-//  // The biases are 32-bits in chip frame in hardware unit scaled by:
-//  // 2^12 (FSR 4g) for accel, 2^15 for gyro, in uT scaled by 2^16 for compass.
-//  const unsigned char accelBiasOne[4] = {0x00, 0x00, 0x10, 0x00};
-//  const unsigned char gyroBiasOne[4] = {0x00, 0x00, 0x80, 0x00};
-//  const unsigned char compassBiasOne[4] = {0x00, 0x01, 0x00, 0x00};
-//  success &= (myICM.writeDMPmems(GYRO_BIAS_X, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(GYRO_BIAS_Y, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(GYRO_BIAS_Z, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(ACCEL_BIAS_X, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(ACCEL_BIAS_Y, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(ACCEL_BIAS_Z, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(CPASS_BIAS_X, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(CPASS_BIAS_Y, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
-//  success &= (myICM.writeDMPmems(CPASS_BIAS_Z, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
-
+  // 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
+  success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Gyro scaling factor
+  const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok);
+  
+  // Configure the Gyro full scale
+  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
+  
   // Enable DMP interrupt
   // This would be the most efficient way of getting the DMP data, instead of polling the FIFO
   //success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
@@ -246,12 +255,12 @@ void setup() {
 
   // Check success
   if( success )
-    SERIAL_PORT.println("DMP enabled!");
+    SERIAL_PORT.println(F("DMP enabled!"));
   else
   {
-    SERIAL_PORT.println("Enable DMP failed!");
+    SERIAL_PORT.println(F("Enable DMP failed!"));
+    SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
   }
-  
 }
 
 void loop()