diff --git a/DMP.md b/DMP.md
index 0a8150a..e476f81 100644
--- a/DMP.md
+++ b/DMP.md
@@ -74,6 +74,7 @@ The DMP data is returned via the FIFO (First In First Out). ```readDMPdataFromFI
 - ```ICM_20948_Stat_FIFONoDataAvail``` if no data or incomplete data is available
 - ```ICM_20948_Stat_Ok``` if a valid frame was read
 - ```ICM_20948_Stat_FIFOMoreDataAvail``` if a valid frame was read _and_ the FIFO contains more (unread) data
+- ```ICM_20948_Stat_FIFOIncompleteData``` if a frame was present in the FIFO but it was incomplete
 
 You can examine the 16-bit ```icm_20948_DMP_data_t data.header``` to see what data the frame contained. ```data.header``` is a bit field; each bit indicates what data is present:
 - **DMP_header_bitmap_Compass_Calibr** (0x0020)
diff --git a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
index cf129ff..f63ff78 100644
--- a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
+++ b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
@@ -11,7 +11,7 @@
  * ** We are grateful to InvenSense for sharing this with us.
  * 
  * ** Important note: by default the DMP functionality is disabled in the library
- * ** as the DMP firmware takes up 14290 Bytes of program memory.
+ * ** as the DMP firmware takes up 14301 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
@@ -24,6 +24,8 @@
  * Distributed as-is; no warranty is given.
  ***************************************************************/
 
+#define QUAT_ANIMATION // Uncomment this line to output data in the correct format for ZaneL's Node.js Quaternion animation tool: https://github.com/ZaneL/quaternion_sensor_3d_nodejs
+
 #include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
 
 //#define USE_SPI       // Uncomment this to use SPI
@@ -48,10 +50,13 @@
 void setup() {
 
   SERIAL_PORT.begin(115200); // Start the serial console
+#ifndef QUAT_ANIMATION
   SERIAL_PORT.println(F("ICM-20948 Example"));
+#endif
 
   delay(100);
 
+#ifndef QUAT_ANIMATION
   while (SERIAL_PORT.available()) // Make sure the serial RX buffer is empty
     SERIAL_PORT.read();
 
@@ -59,6 +64,7 @@ void setup() {
 
   while (!SERIAL_PORT.available()) // Wait for the user to press a key (send any serial character)
     ;
+#endif
 
 #ifdef USE_SPI
     SPI_PORT.begin();
@@ -67,7 +73,9 @@ void setup() {
     WIRE_PORT.setClock(400000);
 #endif
 
+#ifndef QUAT_ANIMATION
   //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+#endif
 
   bool initialized = false;
   while( !initialized ){
@@ -80,17 +88,23 @@ void setup() {
     myICM.begin( WIRE_PORT, AD0_VAL );
 #endif
 
+#ifndef QUAT_ANIMATION
     SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
     SERIAL_PORT.println( myICM.statusString() );
+#endif
     if( myICM.status != ICM_20948_Stat_Ok ){
+#ifndef QUAT_ANIMATION
       SERIAL_PORT.println( F("Trying again...") );
+#endif
       delay(500);
     }else{
       initialized = true;
     }
   }
 
-  SERIAL_PORT.println("Device connected!");
+#ifndef QUAT_ANIMATION
+  SERIAL_PORT.println(F("Device connected!"));
+#endif
 
   // 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".
@@ -103,8 +117,9 @@ 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
-  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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6
+  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
   success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
@@ -136,6 +151,8 @@ void setup() {
 
   // 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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t zero = 0;
   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);
@@ -149,9 +166,9 @@ void setup() {
   // 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 ); // ** Note: comment this line to leave the sample rates at the maximum **
+  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).
+  myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
   
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
   success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
@@ -162,14 +179,24 @@ void setup() {
   // 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
 
+  // Set the Hardware Fix Disable register to 0x48
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fix = 0x48;
+  success &= (myICM.write(AGB0_REG_HW_FIX_DISABLE, &fix, 1) == ICM_20948_Stat_Ok);
+  
+  // Set the Single FIFO Priority Select register to 0xE4
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fifoPrio = 0xE4;
+  success &= (myICM.write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1) == ICM_20948_Stat_Ok);
+  
   // 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
+  // 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};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // 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};
-  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // Write 0x40000 to ACC_SCALE2 DMP register
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // 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.
@@ -178,6 +205,8 @@ 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
+  // 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
@@ -204,26 +233,47 @@ void setup() {
   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 DMP 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);
+  // @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
+  success &= (myICM.setGyroSF(19, 3) == ICM_20948_Stat_Ok); // 19 = 55Hz (see above), 3 = 2000dps (see above)
   
   // Configure the Gyro full scale
-  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  // 2000dps : 2^28
+  // 1000dps : 2^27
+  //  500dps : 2^26
+  //  250dps : 2^25
+  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
-  
+
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
-  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
+  //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz
   success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
-  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
+  //const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
-  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
+  //const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
+  // Configure the Accel Cal Rate
+  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
+  success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[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);
@@ -248,15 +298,22 @@ void setup() {
   // Enable the DMP orientation sensor
   success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
 
+  // Enable any additional sensors / features
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
+
   // Configuring DMP to output data at multiple ODRs:
   // DMP is capable of outputting multiple sensor data at different rates to FIFO.
-  // Set the DMP Output Data Rate for Quat9 to 12Hz.
-  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 12) == ICM_20948_Stat_Ok);
-
-  // Set the DMP Data Ready Status register
-  const uint16_t dsrBits = DMP_Data_ready_Gyro | DMP_Data_ready_Accel | DMP_Data_ready_Secondary_Compass;
-  const unsigned char drsReg[2] = {(const unsigned char)(dsrBits >> 8), (const unsigned char)(dsrBits & 0xFF)};
-  success &= (myICM.writeDMPmems(DATA_RDY_STATUS, 2, &drsReg[0]) == ICM_20948_Stat_Ok);  
+  // Setting value can be calculated as follows:
+  // Value = (DMP running rate / ODR ) - 1
+  // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
 
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
@@ -272,7 +329,11 @@ void setup() {
 
   // Check success
   if( success )
+  {
+#ifndef QUAT_ANIMATION
     SERIAL_PORT.println(F("DMP enabled!"));
+#endif
+  }
   else
   {
     SERIAL_PORT.println(F("Enable DMP failed!"));
@@ -284,8 +345,9 @@ void loop()
 {
   // Read any DMP data waiting in the FIFO
   // Note:
-  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data or incomplete data is available.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available.
   //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete
   //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
   //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
   icm_20948_DMP_data_t data;
@@ -308,10 +370,12 @@ void loop()
       //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy);
 
       // Scale to +/- 1
-      float q1 = ((float)data.Quat9.Data.Q1) / 1073741824.0; // Convert to float. Divide by 2^30
-      float q2 = ((float)data.Quat9.Data.Q2) / 1073741824.0; // Convert to float. Divide by 2^30
-      float q3 = ((float)data.Quat9.Data.Q3) / 1073741824.0; // Convert to float. Divide by 2^30
+      double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q0 = sqrt( 1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
       
+#ifndef QUAT_ANIMATION
       SERIAL_PORT.print(F("Q1:"));
       SERIAL_PORT.print(q1, 3);
       SERIAL_PORT.print(F(" Q2:"));
@@ -320,6 +384,18 @@ void loop()
       SERIAL_PORT.print(q3, 3);
       SERIAL_PORT.print(F(" Accuracy:"));
       SERIAL_PORT.println(data.Quat9.Data.Accuracy);
+#else
+      // Output the Quaternion data in the format expected by ZaneL's Node.js Quaternion animation tool
+      SERIAL_PORT.print(F("{\"quat_w\":"));
+      SERIAL_PORT.print(q0, 3);
+      SERIAL_PORT.print(F(", \"quat_x\":"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(", \"quat_y\":"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(", \"quat_z\":"));
+      SERIAL_PORT.print(q3, 3);
+      SERIAL_PORT.println(F("}"));
+#endif
     }
   }
 
diff --git a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
index fedf03b..9028210 100644
--- a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
+++ b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
@@ -11,7 +11,7 @@
  * ** We are grateful to InvenSense for sharing this with us.
  * 
  * ** Important note: by default the DMP functionality is disabled in the library
- * ** as the DMP firmware takes up 14290 Bytes of program memory.
+ * ** as the DMP firmware takes up 14301 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
@@ -24,6 +24,8 @@
  * Distributed as-is; no warranty is given.
  ***************************************************************/
 
+//#define QUAT_ANIMATION // Uncomment this line to output data in the correct format for ZaneL's Node.js Quaternion animation tool: https://github.com/ZaneL/quaternion_sensor_3d_nodejs
+
 #include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
 
 //#define USE_SPI       // Uncomment this to use SPI
@@ -48,10 +50,13 @@
 void setup() {
 
   SERIAL_PORT.begin(115200); // Start the serial console
+#ifndef QUAT_ANIMATION
   SERIAL_PORT.println(F("ICM-20948 Example"));
+#endif
 
   delay(100);
 
+#ifndef QUAT_ANIMATION
   while (SERIAL_PORT.available()) // Make sure the serial RX buffer is empty
     SERIAL_PORT.read();
 
@@ -59,6 +64,7 @@ void setup() {
 
   while (!SERIAL_PORT.available()) // Wait for the user to press a key (send any serial character)
     ;
+#endif
 
 #ifdef USE_SPI
     SPI_PORT.begin();
@@ -67,7 +73,9 @@ void setup() {
     WIRE_PORT.setClock(400000);
 #endif
 
+#ifndef QUAT_ANIMATION
   //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+#endif
 
   bool initialized = false;
   while( !initialized ){
@@ -80,17 +88,23 @@ void setup() {
     myICM.begin( WIRE_PORT, AD0_VAL );
 #endif
 
+#ifndef QUAT_ANIMATION
     SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
     SERIAL_PORT.println( myICM.statusString() );
+#endif
     if( myICM.status != ICM_20948_Stat_Ok ){
+#ifndef QUAT_ANIMATION
       SERIAL_PORT.println( F("Trying again...") );
+#endif
       delay(500);
     }else{
       initialized = true;
     }
   }
 
+#ifndef QUAT_ANIMATION
   SERIAL_PORT.println(F("Device connected!"));
+#endif
 
   // 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".
@@ -103,8 +117,9 @@ 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
-  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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6
+  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
   success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
@@ -136,6 +151,8 @@ void setup() {
 
   // 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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t zero = 0;
   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);
@@ -149,8 +166,8 @@ void setup() {
   // 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
-  mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz
+  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).
   myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
   
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
@@ -162,14 +179,24 @@ void setup() {
   // 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
 
+  // Set the Hardware Fix Disable register to 0x48
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fix = 0x48;
+  success &= (myICM.write(AGB0_REG_HW_FIX_DISABLE, &fix, 1) == ICM_20948_Stat_Ok);
+  
+  // Set the Single FIFO Priority Select register to 0xE4
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fifoPrio = 0xE4;
+  success &= (myICM.write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1) == ICM_20948_Stat_Ok);
+  
   // 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
+  // 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};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // 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};
-  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // Write 0x40000 to ACC_SCALE2 DMP register
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // 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.
@@ -178,6 +205,8 @@ 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
+  // 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
@@ -204,26 +233,47 @@ void setup() {
   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 DMP 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);
+  // @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
+  success &= (myICM.setGyroSF(19, 3) == ICM_20948_Stat_Ok); // 19 = 55Hz (see above), 3 = 2000dps (see above)
   
   // Configure the Gyro full scale
-  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  // 2000dps : 2^28
+  // 1000dps : 2^27
+  //  500dps : 2^26
+  //  250dps : 2^25
+  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
-  
+
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
-  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
+  //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz
   success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
-  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
+  //const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
-  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
+  //const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
+  // Configure the Accel Cal Rate
+  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
+  success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[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);
@@ -248,10 +298,22 @@ void setup() {
   // Enable the DMP Game Rotation Vector sensor
   success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);
 
+  // Enable any additional sensors / features
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
+
   // Configuring DMP to output data at multiple ODRs:
   // DMP is capable of outputting multiple sensor data at different rates to FIFO.
-  // Set the DMP Output Data Rate for Quat6 to 12Hz.
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 12) == ICM_20948_Stat_Ok);
+  // Setting value can be calculated as follows:
+  // Value = (DMP running rate / ODR ) - 1
+  // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
 
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
@@ -267,7 +329,11 @@ void setup() {
 
   // Check success
   if( success )
+  {
+#ifndef QUAT_ANIMATION
     SERIAL_PORT.println(F("DMP enabled!"));
+#endif
+  }
   else
   {
     SERIAL_PORT.println(F("Enable DMP failed!"));
@@ -279,8 +345,9 @@ void loop()
 {
   // Read any DMP data waiting in the FIFO
   // Note:
-  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data or incomplete data is available.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available.
   //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete
   //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
   //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
   icm_20948_DMP_data_t data;
@@ -307,9 +374,18 @@ void loop()
       double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
       double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
       
-      //SERIAL_PORT.printf("Q1:%.3f Q2:%.3f Q3:%.3f\r\n", q1, q2, q3);
+/*
+      SERIAL_PORT.print(F("Q1:"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(" Q2:"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(" Q3:"));
+      SERIAL_PORT.println(q3, 3);
+*/
 
       // Convert the quaternions to Euler angles (roll, pitch, yaw)
+      // https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles&section=8#Source_code_2
+      
       double q0 = sqrt( 1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
 
       double q2sqr = q2 * q2;
@@ -330,12 +406,25 @@ void loop()
       double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
       double yaw = atan2(t3, t4) * 180.0 / PI;
 
+#ifndef QUAT_ANIMATION
       SERIAL_PORT.print(F("Roll:"));
       SERIAL_PORT.print(roll, 1);
       SERIAL_PORT.print(F(" Pitch:"));
       SERIAL_PORT.print(pitch, 1);
       SERIAL_PORT.print(F(" Yaw:"));
       SERIAL_PORT.println(yaw, 1);
+#else
+      // Output the Quaternion data in the format expected by ZaneL's Node.js Quaternion animation tool
+      SERIAL_PORT.print(F("{\"quat_w\":"));
+      SERIAL_PORT.print(q0, 3);
+      SERIAL_PORT.print(F(", \"quat_x\":"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(", \"quat_y\":"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(", \"quat_z\":"));
+      SERIAL_PORT.print(q3, 3);
+      SERIAL_PORT.println(F("}"));
+#endif
     }
   }
 
diff --git a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
index 7f993ac..7a1740c 100644
--- a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
+++ b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
@@ -11,7 +11,7 @@
  * ** We are grateful to InvenSense for sharing this with us.
  * 
  * ** Important note: by default the DMP functionality is disabled in the library
- * ** as the DMP firmware takes up 14290 Bytes of program memory.
+ * ** as the DMP firmware takes up 14301 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
@@ -103,8 +103,9 @@ 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
-  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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6
+  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
   success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
@@ -136,6 +137,8 @@ void setup() {
 
   // 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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t zero = 0;
   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);
@@ -149,9 +152,9 @@ void setup() {
   // 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
-  mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz
-  //myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
+  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).
+  myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
   
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
   success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
@@ -162,14 +165,24 @@ void setup() {
   // 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
 
+  // Set the Hardware Fix Disable register to 0x48
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fix = 0x48;
+  success &= (myICM.write(AGB0_REG_HW_FIX_DISABLE, &fix, 1) == ICM_20948_Stat_Ok);
+  
+  // Set the Single FIFO Priority Select register to 0xE4
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fifoPrio = 0xE4;
+  success &= (myICM.write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1) == ICM_20948_Stat_Ok);
+  
   // 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
+  // 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};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // 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};
-  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // Write 0x40000 to ACC_SCALE2 DMP register
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // 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.
@@ -178,6 +191,8 @@ 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
+  // 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
@@ -204,26 +219,47 @@ void setup() {
   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 DMP 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);
+  // @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
+  success &= (myICM.setGyroSF(19, 3) == ICM_20948_Stat_Ok); // 19 = 55Hz (see above), 3 = 2000dps (see above)
   
   // Configure the Gyro full scale
-  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  // 2000dps : 2^28
+  // 1000dps : 2^27
+  //  500dps : 2^26
+  //  250dps : 2^25
+  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
-  
+
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
-  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
+  //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz
   success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
-  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
+  //const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
-  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
+  //const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
   
+  // Configure the Accel Cal Rate
+  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
+  success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[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);
@@ -250,8 +286,10 @@ void setup() {
 
   // Configuring DMP to output data at multiple ODRs:
   // DMP is capable of outputting multiple sensor data at different rates to FIFO.
-  // Set the DMP Output Data Rates to 2Hz
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 2) == ICM_20948_Stat_Ok); // ** Note: comment this line to leave the data rate at the maximum **
+  // Setting value can be calculated as follows:
+  // Value = (DMP running rate / ODR ) - 1
+  // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
 
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
@@ -267,7 +305,9 @@ void setup() {
 
   // Check success
   if( success )
+  {
     SERIAL_PORT.println(F("DMP enabled!"));
+  }
   else
   {
     SERIAL_PORT.println(F("Enable DMP failed!"));
@@ -279,8 +319,9 @@ void loop()
 {
   // Read any DMP data waiting in the FIFO
   // Note:
-  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data or incomplete data is available.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available.
   //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete
   //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
   //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
   icm_20948_DMP_data_t data;
diff --git a/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino
new file mode 100644
index 0000000..4c9eaec
--- /dev/null
+++ b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino
@@ -0,0 +1,419 @@
+/****************************************************************
+ * 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
+ * Based on original code by:
+ * Owen Lyke @ SparkFun Electronics
+ * Original Creation Date: April 17 2019
+ * 
+ * ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
+ * ** We are grateful to InvenSense for sharing this with us.
+ * 
+ * ** Important note: by default the DMP functionality is disabled in the library
+ * ** as the DMP firmware takes up 14301 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.
+ *
+ * Distributed as-is; no warranty is given.
+ ***************************************************************/
+
+#include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
+
+//#define USE_SPI       // Uncomment this to use SPI
+
+#define SERIAL_PORT Serial
+
+#define SPI_PORT SPI    // Your desired SPI port.       Used only when "USE_SPI" is defined
+#define CS_PIN 2        // Which pin you connect CS to. Used only when "USE_SPI" is defined
+
+#define WIRE_PORT Wire  // Your desired Wire port.      Used when "USE_SPI" is not defined
+#define AD0_VAL   1     // The value of the last bit of the I2C address.
+                        // On the SparkFun 9DoF IMU breakout the default is 1, and when
+                        // the ADR jumper is closed the value becomes 0
+
+#ifdef USE_SPI
+  ICM_20948_SPI myICM;  // If using SPI create an ICM_20948_SPI object
+#else
+  ICM_20948_I2C myICM;  // Otherwise create an ICM_20948_I2C object
+#endif
+
+
+void setup() {
+
+  SERIAL_PORT.begin(115200); // Start the serial console
+  SERIAL_PORT.println(F("ICM-20948 Example"));
+
+  delay(100);
+
+  while (SERIAL_PORT.available()) // Make sure the serial RX buffer is empty
+    SERIAL_PORT.read();
+
+  SERIAL_PORT.println(F("Press any key to continue..."));
+
+  while (!SERIAL_PORT.available()) // Wait for the user to press a key (send any serial character)
+    ;
+
+#ifdef USE_SPI
+    SPI_PORT.begin();
+#else
+    WIRE_PORT.begin();
+    WIRE_PORT.setClock(400000);
+#endif
+
+  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+
+  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
+    myICM.begin( WIRE_PORT, AD0_VAL );
+#endif
+
+    SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
+    SERIAL_PORT.println( myICM.statusString() );
+    if( myICM.status != ICM_20948_Stat_Ok ){
+      SERIAL_PORT.println( F("Trying again...") );
+      delay(500);
+    }else{
+      initialized = true;
+    }
+  }
+
+  SERIAL_PORT.println(F("Device connected!"));
+
+  // 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".
+
+  bool success = true; // Use success to show if the configuration was successful
+
+  // 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
+
+  // 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
+  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
+  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
+  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
+  // If we see this interrupt, we'll need to reset the FIFO
+  //success &= (myICM.intEnableOverflowFIFO( 0x1F ) == ICM_20948_Stat_Ok); // 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
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t zero = 0;
+  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 = 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).
+  myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
+  
+  // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+  success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
+
+  // 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
+
+  // Set the Hardware Fix Disable register to 0x48
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fix = 0x48;
+  success &= (myICM.write(AGB0_REG_HW_FIX_DISABLE, &fix, 1) == ICM_20948_Stat_Ok);
+  
+  // Set the Single FIFO Priority Select register to 0xE4
+  success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0
+  uint8_t fifoPrio = 0xE4;
+  success &= (myICM.write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1) == ICM_20948_Stat_Ok);
+  
+  // 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};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // 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};
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // 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
+  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, &mountMultiplierMinus[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, &mountMultiplierMinus[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 DMP 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);
+  // @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
+  success &= (myICM.setGyroSF(19, 3) == ICM_20948_Stat_Ok); // 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
+  success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
+
+  // 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}; // InvenSense Nucleo example uses 225Hz
+  success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
+  
+  // 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] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
+  
+  // 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] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
+  
+  // Configure the Accel Cal Rate
+  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
+  success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[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);
+
+  // DMP sensor options are defined in ICM_20948_DMP.h
+  //    INV_ICM20948_SENSOR_ACCELEROMETER               (16-bit accel)
+  //    INV_ICM20948_SENSOR_GYROSCOPE                   (16-bit gyro + 32-bit calibrated gyro)
+  //    INV_ICM20948_SENSOR_RAW_ACCELEROMETER           (16-bit accel)
+  //    INV_ICM20948_SENSOR_RAW_GYROSCOPE               (16-bit gyro + 32-bit calibrated gyro)
+  //    INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED (16-bit compass)
+  //    INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED      (16-bit gyro)
+  //    INV_ICM20948_SENSOR_STEP_DETECTOR               (Pedometer Step Detector)
+  //    INV_ICM20948_SENSOR_STEP_COUNTER                (Pedometer Step Detector)
+  //    INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR        (32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_ROTATION_VECTOR             (32-bit 9-axis quaternion + heading accuracy)
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR (32-bit Geomag RV + heading accuracy)
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD           (32-bit calibrated compass)
+  //    INV_ICM20948_SENSOR_GRAVITY                     (32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_LINEAR_ACCELERATION         (16-bit accel + 32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_ORIENTATION                 (32-bit 9-axis quaternion + heading accuracy)
+
+  // Enable the DMP Game Rotation Vector sensor
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);
+
+  // Enable additional sensors / features
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
+
+  // Configuring DMP to output data at multiple ODRs:
+  // DMP is capable of outputting multiple sensor data at different rates to FIFO.
+  // Setting value can be calculated as follows:
+  // Value = (DMP running rate / ODR ) - 1
+  // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 10) == ICM_20948_Stat_Ok); // Set to 5Hz
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 54) == ICM_20948_Stat_Ok); // Set to 1Hz
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 54) == ICM_20948_Stat_Ok); // Set to 1Hz
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 54) == ICM_20948_Stat_Ok); // Set to 1Hz
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 54) == ICM_20948_Stat_Ok); // Set to 1Hz
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 54) == ICM_20948_Stat_Ok); // Set to 1Hz
+
+  // Enable the FIFO
+  success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
+
+  // Enable the DMP
+  success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
+
+  // Reset DMP
+  success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
+
+  // Reset FIFO
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // Check success
+  if( success )
+  {
+    SERIAL_PORT.println(F("DMP enabled!"));
+  }
+  else
+  {
+    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()
+{
+  // Read any DMP data waiting in the FIFO
+  // Note:
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available.
+  //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
+  icm_20948_DMP_data_t data;
+  myICM.readDMPdataFromFIFO(&data);
+
+  if(( myICM.status == ICM_20948_Stat_Ok ) || ( myICM.status == ICM_20948_Stat_FIFOMoreDataAvail )) // Was valid data available?
+  {
+    //SERIAL_PORT.print(F("Received data! Header: 0x")); // Print the header in HEX so we can see what data is arriving in the FIFO
+    //if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
+    //if ( data.header < 0x100) SERIAL_PORT.print( "0" );
+    //if ( data.header < 0x10) SERIAL_PORT.print( "0" );
+    //SERIAL_PORT.println( data.header, HEX );
+
+    if ( (data.header & DMP_header_bitmap_Quat9) > 0 ) // Check for orientation data (Quat9)
+    {
+      // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1.
+      // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.
+      // The quaternion data is scaled by 2^30.
+
+      //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy);
+
+      // Scale to +/- 1
+      double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
+      
+      SERIAL_PORT.print(F("Q1:"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(" Q2:"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(" Q3:"));
+      SERIAL_PORT.print(q3, 3);
+      SERIAL_PORT.print(F(" Accuracy:"));
+      SERIAL_PORT.println(data.Quat9.Data.Accuracy);
+    }
+
+    if ( (data.header & DMP_header_bitmap_Accel) > 0 ) // Check for Accel
+    {
+      float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data
+      float acc_y = (float)data.Raw_Accel.Data.Y; 
+      float acc_z = (float)data.Raw_Accel.Data.Z; 
+    
+      SERIAL_PORT.print(F("Accel: X:"));
+      SERIAL_PORT.print(acc_x);
+      SERIAL_PORT.print(F(" Y:"));
+      SERIAL_PORT.print(acc_y);
+      SERIAL_PORT.print(F(" Z:"));
+      SERIAL_PORT.println(acc_z);
+    }
+
+    if ( (data.header & DMP_header_bitmap_Gyro) > 0 ) // Check for Gyro
+    {
+      float x = (float)data.Raw_Gyro.Data.X; // Extract the raw gyro data
+      float y = (float)data.Raw_Gyro.Data.Y; 
+      float z = (float)data.Raw_Gyro.Data.Z; 
+    
+      SERIAL_PORT.print(F("Gyro: X:"));
+      SERIAL_PORT.print(x);
+      SERIAL_PORT.print(F(" Y:"));
+      SERIAL_PORT.print(y);
+      SERIAL_PORT.print(F(" Z:"));
+      SERIAL_PORT.println(z);
+    }
+
+    if ( (data.header & DMP_header_bitmap_Compass) > 0 ) // Check for Compass
+    {
+      float x = (float)data.Compass.Data.X; // Extract the compass data
+      float y = (float)data.Compass.Data.Y; 
+      float z = (float)data.Compass.Data.Z; 
+    
+      SERIAL_PORT.print(F("Compass: X:"));
+      SERIAL_PORT.print(x);
+      SERIAL_PORT.print(F(" Y:"));
+      SERIAL_PORT.print(y);
+      SERIAL_PORT.print(F(" Z:"));
+      SERIAL_PORT.println(z);
+    }
+
+  }
+
+  if ( myICM.status != ICM_20948_Stat_FIFOMoreDataAvail ) // If more data is available then we should read it right away - and not delay
+  {
+    delay(10);
+  }
+}
diff --git a/keywords.txt b/keywords.txt
index afb7973..5c36cd5 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -94,6 +94,7 @@ writeDMPmems	KEYWORD2
 readDMPmems	KEYWORD2
 setDMPODRrate	KEYWORD2
 readDMPdataFromFIFO	KEYWORD2
+setGyroSF	KEYWORD2
 begin	KEYWORD2
 
 #######################################
@@ -122,6 +123,7 @@ ICM_20948_Stat_SensorNotSupported	LITERAL1
 ICM_20948_Stat_DMPNotSupported	LITERAL1
 ICM_20948_Stat_DMPVerifyFail	LITERAL1
 ICM_20948_Stat_FIFONoDataAvail	LITERAL1
+ICM_20948_Stat_FIFOIncompleteData	LITERAL1
 ICM_20948_Stat_FIFOMoreDataAvail	LITERAL1
 ICM_20948_Stat_UnrecognisedDMPHeader	LITERAL1
 ICM_20948_Stat_UnrecognisedDMPHeader2	LITERAL1
diff --git a/library.properties b/library.properties
index 9d12c2e..40f5801 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library
-version=1.2.1
+version=1.2.2
 author=SparkFun Electronics <techsupport@sparkfun.com>
 maintainer=SparkFun Electronics <sparkfun.com>
 sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™).
diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 16c81d1..97ff625 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -116,6 +116,9 @@ void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat)
     case ICM_20948_Stat_FIFONoDataAvail:
         debugPrint(F("No FIFO Data Available"));
         break;
+    case ICM_20948_Stat_FIFOIncompleteData:
+        debugPrint(F("DMP data in FIFO was incomplete"));
+        break;
     case ICM_20948_Stat_FIFOMoreDataAvail:
         debugPrint(F("More FIFO Data Available"));
         break;
@@ -292,6 +295,9 @@ const char *ICM_20948::statusString(ICM_20948_Status_e stat)
     case ICM_20948_Stat_FIFONoDataAvail:
         return "No FIFO Data Available";
         break;
+    case ICM_20948_Stat_FIFOIncompleteData:
+        return "DMP data in FIFO was incomplete";
+        break;
     case ICM_20948_Stat_FIFOMoreDataAvail:
         return "More FIFO Data Available";
         break;
@@ -1026,6 +1032,11 @@ ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, b
   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);
+    debugPrint(F("ICM_20948::enableDMPSensor:  _enabled_Android_0: "));
+    debugPrintf((int)_device._enabled_Android_0);
+    debugPrint(F("  _enabled_Android_1: "));
+    debugPrintf((int)_device._enabled_Android_1);
+    debugPrintln(F(""));
     return status;
   }
   return ICM_20948_Stat_DMPNotSupported;
@@ -1036,6 +1047,11 @@ ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor
   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);
+    debugPrint(F("ICM_20948::enableDMPSensorInt:  _enabled_Android_intr_0: "));
+    debugPrintf((int)_device._enabled_Android_intr_0);
+    debugPrint(F("  _enabled_Android_intr_1: "));
+    debugPrintf((int)_device._enabled_Android_intr_1);
+    debugPrintln(F(""));
     return status;
   }
   return ICM_20948_Stat_DMPNotSupported;
@@ -1061,7 +1077,7 @@ ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int lengt
   return ICM_20948_Stat_DMPNotSupported;
 }
 
-ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int rate)
+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?
   {
@@ -1070,11 +1086,6 @@ ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int
   	// Value = (DMP running rate (225Hz) / ODR ) - 1
   	// E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8.
 
-    if (rate <= 0) // Check rate is valid
-        rate = 1;
-    if (rate > 225)
-        rate = 225;
-    uint16_t interval = (225 / rate) - 1;
     status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval);
     return status;
   }
@@ -1091,6 +1102,21 @@ ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data)
   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);
+    debugPrint(F("ICM_20948::setGyroSF:  pll: "));
+    debugPrintf((int)_device._gyroSFpll);
+    debugPrint(F("  Gyro SF is: "));
+    debugPrintf((int)_device._gyroSF);
+    debugPrintln(F(""));
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
+}
+
 // I2C
 ICM_20948_I2C::ICM_20948_I2C()
 {
@@ -1140,6 +1166,12 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
     _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.
@@ -1321,6 +1353,12 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
     _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.
diff --git a/src/ICM_20948.h b/src/ICM_20948.h
index 751c0ef..17b7cf2 100644
--- a/src/ICM_20948.h
+++ b/src/ICM_20948.h
@@ -180,23 +180,23 @@ class ICM_20948
     //  Reset FIFO
     //  Reset DMP
     //  Enable DMP interrupt
-    //  Configuring DMP to output data to FIFO: set DATA_OUT_CTL1 and DATA_INTR_CTL
+    //  Configuring DMP to output data to FIFO: set DATA_OUT_CTL1, DATA_OUT_CTL2, DATA_INTR_CTL and MOTION_EVENT_CTL
     //  Configuring DMP to output data at multiple ODRs
+    //  Configure DATA_RDY_STATUS
+    //  Configuring Accel calibration
+    //  Configuring Compass calibration
+    //  Configuring Gyro gain
+    //  Configuring Accel gain
 
     // To Do:
-    //  Configure DATA_RDY_STATUS
-    //  Additional FIFO output control: DATA_OUT_CTL2, FIFO_WATERMARK, BM_BATCH_MASK, BM_BATCH_CNTR, BM_BATCH_THLD
-    //  Configuring DMP features: MOTION_EVENT_CTL, PED_STD_STEPCTR, PED_STD_TIMECTR
+    //  Additional FIFO output control: FIFO_WATERMARK, BM_BATCH_MASK, BM_BATCH_CNTR, BM_BATCH_THLD
+    //  Configuring DMP features: PED_STD_STEPCTR, PED_STD_TIMECTR
     //  Enabling Activity Recognition (BAC) feature
     //  Enabling Significant Motion Detect (SMD) feature
     //  Enabling Tilt Detector feature
     //  Enabling Pick Up Gesture feature
     //  Enabling Fsync detection feature
-    //  Configuring Accel calibration
-    //  Configuring Compass calibration
-    //  Configuring Gyro gain
-    //  Configuring Accel gain
-    //  Biases
+    //  Biases?
 
     ICM_20948_Status_e enableDMP(bool enable = true);
     ICM_20948_Status_e resetDMP(void);
@@ -206,8 +206,9 @@ class ICM_20948
     ICM_20948_Status_e enableDMPSensorInt(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);
-    ICM_20948_Status_e setDMPODRrate(enum DMP_ODR_Registers odr_reg, int rate);
+    ICM_20948_Status_e setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval);
     ICM_20948_Status_e readDMPdataFromFIFO(icm_20948_DMP_data_t *data);
+    ICM_20948_Status_e setGyroSF(unsigned char div, int gyro_level);
 
 };
 
diff --git a/src/util/ICM_20948_C.c b/src/util/ICM_20948_C.c
index 87058a7..1522716 100644
--- a/src/util/ICM_20948_C.c
+++ b/src/util/ICM_20948_C.c
@@ -1300,7 +1300,7 @@ ICM_20948_Status_e inv_icm20948_write_mems(ICM_20948_Device_t *pdev, unsigned sh
 ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, unsigned char *data)
 {
 	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
-	unsigned int bytesWritten = 0;
+	unsigned int bytesRead = 0;
 	unsigned int thisLen;
 	unsigned char lBankSelected;
 	unsigned char lStartAddrSelected;
@@ -1318,13 +1318,17 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 
 	lBankSelected = (reg >> 8);
 
-	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
-	if (result != ICM_20948_Stat_Ok)
+	if (lBankSelected != pdev->_last_mems_bank)
 	{
-			return result;
+			pdev->_last_mems_bank = lBankSelected;
+			result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
+			if (result != ICM_20948_Stat_Ok)
+			{
+					return result;
+			}
 	}
 
-	while (bytesWritten < length)
+	while (bytesRead < length)
 	{
 		lStartAddrSelected = (reg & 0xff);
 
@@ -1339,20 +1343,20 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 				return result;
 		}
 
-		if (length-bytesWritten <= INV_MAX_SERIAL_READ)
-				thisLen = length-bytesWritten;
+		if (length-bytesRead <= INV_MAX_SERIAL_READ)
+				thisLen = length-bytesRead;
 		else
 				thisLen = INV_MAX_SERIAL_READ;
 
 		/* Read data */
 
-		result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesWritten], thisLen);
+		result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesRead], thisLen);
 		if (result != ICM_20948_Stat_Ok)
 		{
 				return result;
 		}
 
-		bytesWritten += thisLen;
+		bytesRead += thisLen;
 		reg += thisLen;
 	}
 
@@ -1361,7 +1365,7 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 
 ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum DMP_ODR_Registers odr_reg, uint16_t interval)
 {
-	// Set the ODR registersand clear the ODR counter
+	// Set the ODR registers and clear the ODR counter
 
 	// 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:
@@ -1480,25 +1484,82 @@ ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev,
 
 ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, int state)
 {
-		// TO DO: figure out how to disable the sensor if state is 0
-
 		ICM_20948_Status_e result = ICM_20948_Stat_Ok;
 
-		uint16_t inv_event_control = 0;
-		uint16_t data_rdy_status = 0;
+		uint16_t inv_event_control = 0; // Use this to store the value for MOTION_EVENT_CTL
+		uint16_t data_rdy_status = 0; // Use this to store the value for DATA_RDY_STATUS
 
 		if (pdev->_dmp_firmware_available == false)
-				return ICM_20948_Stat_DMPNotSupported;
+				return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
 
-		uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
+		uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
 
-		if (androidSensor > ANDROID_SENSOR_NUM_MAX)
-				return ICM_20948_Stat_SensorNotSupported;
+		if (androidSensor >= ANDROID_SENSOR_NUM_MAX)
+				return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported (TO DO: Support B2S etc)
 
+		// Convert the Android sensor into a bit mask for DATA_OUT_CTL1
 		uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
-
 		if (delta == 0xFFFF)
-				return ICM_20948_Stat_SensorNotSupported;
+				return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
+
+		// Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_0 / _enabled_Android_1
+		unsigned long androidSensorAsBitMask;
+		if (androidSensor < 32) // Sensors 0-31
+		{
+				androidSensorAsBitMask = 1L << androidSensor;
+				if (state == 0) // Should we disable the sensor?
+				{
+						pdev->_enabled_Android_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
+				}
+				else
+				{
+						pdev->_enabled_Android_0 |= androidSensorAsBitMask; // Set the bit to enable the sensor
+				}
+		}
+		else // Sensors 32-
+		{
+				androidSensorAsBitMask = 1L << (androidSensor - 32);
+				if (state == 0) // Should we disable the sensor?
+				{
+						pdev->_enabled_Android_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
+				}
+				else
+				{
+						pdev->_enabled_Android_1 |= androidSensorAsBitMask; // Set the bit to enable the sensor
+				}
+		}
+
+		// Now we know androidSensor is valid, reconstruct the value for DATA_OUT_CTL1 from _enabled_Android_0 and _enabled_Android_0
+		delta = 0; // Clear delta
+		for (int i = 0; i < 32; i++)
+		{
+				androidSensorAsBitMask = 1L << i;
+				if ((pdev->_enabled_Android_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) is enabled
+				{
+						delta |= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
+				}
+				if ((pdev->_enabled_Android_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) is enabled
+				{
+						delta |= inv_androidSensor_to_control_bits[i+32]; // If it is, or the required bits into delta
+				}
+				// Also check which bits need to be set in the Data Ready Status and Motion Event Control registers
+				// Compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
+				if (((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0))
+				{
+						data_rdy_status |= DMP_Data_ready_Accel;
+						inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
+				}
+				if (((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0))
+				{
+						data_rdy_status |= DMP_Data_ready_Gyro;
+						inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
+				}
+				if (((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0))
+				{
+						data_rdy_status |= DMP_Data_ready_Secondary_Compass;
+						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
+				}
+		}
 
 		result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
 		if (result != ICM_20948_Stat_Ok)
@@ -1512,7 +1573,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 				return result;
 		}
 
-		// Check if Accel, Gyro/Gyro_Calibr or Compass_Calibr/Quat9/GeoMag/Compass were enabled. If they were then we need to request the accuracy data via header2.
+		// Check if Accel, Gyro/Gyro_Calibr or Compass_Calibr/Quat9/GeoMag/Compass are to be enabled. If they are then we need to request the accuracy data via header2.
 		uint16_t delta2 = 0;
 		if ((delta & DMP_Data_Output_Control_1_Accel) > 0)
 		{
@@ -1548,47 +1609,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 				return result;
 		}
 
-		// Check which bits need to be set in the Data Ready Status and Motion Event Control registers
-		// Convert androidSensor into a bit mask and compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
-		unsigned long androidSensorAsBitMask;
-		if (androidSensor < 32)
-		{
-				androidSensorAsBitMask = 1L << androidSensor;
-				if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Accel;
-						inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
-				}
-				if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Gyro;
-						inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
-				}
-				if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Secondary_Compass;
-						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
-				}
-		}
-		else
-		{
-				androidSensorAsBitMask = 1L << (androidSensor - 32);
-				if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Accel;
-						inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
-				}
-				if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Gyro;
-						inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
-				}
-				if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0)
-				{
-						data_rdy_status |= DMP_Data_ready_Secondary_Compass;
-						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
-				}
-		}
+		// Set the DATA_RDY_STATUS register
 		data_output_control_reg[0] = (unsigned char)(data_rdy_status >> 8);
     data_output_control_reg[1] = (unsigned char)(data_rdy_status & 0xff);
 		result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
@@ -1611,6 +1632,8 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 		{
 				inv_event_control |= DMP_Motion_Event_Control_Geomag;
 		}
+
+		// Set the MOTION_EVENT_CTL register
 		data_output_control_reg[0] = (unsigned char)(inv_event_control >> 8);
     data_output_control_reg[1] = (unsigned char)(inv_event_control & 0xff);
 		result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
@@ -1631,20 +1654,59 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev,
 		ICM_20948_Status_e result = ICM_20948_Stat_Ok;
 
 		if (pdev->_dmp_firmware_available == false)
-				return ICM_20948_Stat_DMPNotSupported;
+				return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
 
-		uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
+		uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
 
 		if (androidSensor > ANDROID_SENSOR_NUM_MAX)
-				return ICM_20948_Stat_SensorNotSupported;
+				return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
 
+		// Convert the Android sensor into a bit mask for DATA_OUT_CTL1
 		uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
-
 		if (delta == 0xFFFF)
-				return ICM_20948_Stat_SensorNotSupported;
+				return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
+
+		// Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_intr_0 / _enabled_Android_intr_1
+		unsigned long androidSensorAsBitMask;
+		if (androidSensor < 32) // Sensors 0-31
+		{
+				androidSensorAsBitMask = 1L << androidSensor;
+				if (state == 0) // Should we disable the sensor interrupt?
+				{
+						pdev->_enabled_Android_intr_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
+				}
+				else
+				{
+						pdev->_enabled_Android_intr_0 |= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
+				}
+		}
+		else // Sensors 32-
+		{
+				androidSensorAsBitMask = 1L << (androidSensor - 32);
+				if (state == 0) // Should we disable the sensor?
+				{
+						pdev->_enabled_Android_intr_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
+				}
+				else
+				{
+						pdev->_enabled_Android_intr_1 |= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
+				}
+		}
 
-		if (state == 0)
-				delta = 0;
+		// Now we know androidSensor is valid, reconstruct the value for DATA_INTR_CTL from _enabled_Android_intr_0 and _enabled_Android_intr_0
+		delta = 0; // Clear delta
+		for (int i = 0; i < 32; i++)
+		{
+				androidSensorAsBitMask = 1L << i;
+				if ((pdev->_enabled_Android_intr_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) interrupt is enabled
+				{
+						delta |= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
+				}
+				if ((pdev->_enabled_Android_intr_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) interrupt is enabled
+				{
+						delta |= inv_androidSensor_to_control_bits[i+32]; // If it is, or the required bits into delta
+				}
+		}
 
 		unsigned char data_intr_ctl[2];
 
@@ -1663,7 +1725,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev,
 				return result;
 		}
 
-		// Write the sensor control bits into memory address DATA_OUT_CTL1
+		// Write the interrupt control bits into memory address DATA_INTR_CTL
 		result = inv_icm20948_write_mems(pdev, DATA_INTR_CTL, 2, (const unsigned char *)&data_intr_ctl);
 
 		// result = ICM_20948_low_power(pdev, true); // Put chip into low power state
@@ -1714,7 +1776,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Header2_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if no header2 is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if no header2 is available
 			// Read the header (2 bytes)
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Header2_Bytes);
@@ -1737,7 +1799,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Raw_Accel_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Raw_Accel_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1757,7 +1819,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes))
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes));
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1777,7 +1839,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Compass_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1797,7 +1859,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_ALS_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_ALS_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1817,7 +1879,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Quat6_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat6_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1837,7 +1899,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Quat9_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat9_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1857,7 +1919,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_PQuat6_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_PQuat6_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1877,7 +1939,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Geomag_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Geomag_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1897,7 +1959,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Pressure_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pressure_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1910,6 +1972,10 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 
 	if ((data->header & DMP_header_bitmap_Gyro_Calibr) > 0) // case DMP_header_bitmap_Gyro_Calibr:
 	{
+			// lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes is not supported
+			// and looking at DMP frames which have the Gyro_Calibr bit set, that certainly seems to be true.
+			// So, we'll skip this...:
+			/*
 			if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes) // Check if we need to read the FIFO count again
 			{
 					result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
@@ -1917,7 +1983,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Calibr_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1926,6 +1992,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 					data->Gyro_Calibr.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
 			}
 			fifo_count -= icm_20948_DMP_Gyro_Calibr_Bytes; // Decrement the count
+			*/
 	}
 
 	if ((data->header & DMP_header_bitmap_Compass_Calibr) > 0) // case DMP_header_bitmap_Compass_Calibr:
@@ -1937,7 +2004,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Compass_Calibr_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Calibr_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1957,7 +2024,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Step_Detector_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Step_Detector_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -1981,7 +2048,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Accel_Accuracy_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Accel_Accuracy_Bytes);
 			if (result != ICM_20948_Stat_Ok)
@@ -2003,7 +2070,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Gyro_Accuracy_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Accuracy_Bytes);
 			if (result != ICM_20948_Stat_Ok)
@@ -2025,7 +2092,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Compass_Accuracy_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Accuracy_Bytes);
 			if (result != ICM_20948_Stat_Ok)
@@ -2040,6 +2107,9 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 
 	if ((data->header2 & DMP_header2_bitmap_Fsync) > 0) // case DMP_header2_bitmap_Fsync:
 	{
+			// lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes is not supported.
+			// So, we'll skip this just in case...:
+			/*
 			if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes) // Check if we need to read the FIFO count again
 			{
 					result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
@@ -2047,7 +2117,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Fsync_Detection_Bytes);
 			if (result != ICM_20948_Stat_Ok)
@@ -2058,6 +2128,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 			}
 			data->Fsync_Delay_Time = aShort;
 			fifo_count -= icm_20948_DMP_Fsync_Detection_Bytes; // Decrement the count
+			*/
 	}
 
 	if ((data->header2 & DMP_header2_bitmap_Pickup) > 0) // case DMP_header2_bitmap_Pickup:
@@ -2069,7 +2140,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Pickup_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			aShort = 0;
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pickup_Bytes);
 			if (result != ICM_20948_Stat_Ok)
@@ -2091,7 +2162,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 						return result;
 		}
 		if (fifo_count < icm_20948_DMP_Activity_Recognition_Bytes)
-				return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+				return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 		result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Activity_Recognition_Bytes);
 		if (result != ICM_20948_Stat_Ok)
 				return result;
@@ -2111,7 +2182,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 							return result;
 			}
 			if (fifo_count < icm_20948_DMP_Secondary_On_Off_Bytes)
-					return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+					return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 			result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Secondary_On_Off_Bytes);
 			if (result != ICM_20948_Stat_Ok)
 					return result;
@@ -2130,7 +2201,7 @@ ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_2094
 					return result;
 	}
 	if (fifo_count < icm_20948_DMP_Footer_Bytes)
-			return ICM_20948_Stat_FIFONoDataAvail; // Bail if not enough data is available
+			return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
 	aShort = 0;
 	result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Footer_Bytes);
 	if (result != ICM_20948_Stat_Ok)
@@ -2201,3 +2272,61 @@ enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor)
 	default:                                              return INV_ICM20948_SENSOR_MAX;
 	}
 }
+
+ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level)
+{
+	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+
+	if (pdev->_dmp_firmware_available == false)
+			return ICM_20948_Stat_DMPNotSupported;
+
+	// gyro_level should be set to 4 regardless of fullscale, due to the addition of API dmp_icm20648_set_gyro_fsr()
+	gyro_level = 4;
+
+	// First read the TIMEBASE_CORRECTION_PLL register from Bank 1
+	int8_t pll; // Signed. Typical value is 0x18
+	result = ICM_20948_set_bank(pdev, 1);
+	result = ICM_20948_execute_r(pdev, AGB1_REG_TIMEBASE_CORRECTION_PLL, (uint8_t *)&pll, 1);
+	if (result != ICM_20948_Stat_Ok)
+	{
+			return result;
+	}
+
+	pdev->_gyroSFpll = pll; // Record the PLL value so we can debug print it
+
+	// Now calculate the Gyro SF using code taken from the InvenSense example (inv_icm20948_set_gyro_sf)
+
+	long gyro_sf;
+
+	unsigned long long const MagicConstant = 264446880937391LL;
+	unsigned long long const MagicConstantScale = 100000LL;
+	unsigned long long ResultLL;
+
+	if (pll & 0x80) {
+		ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 - (pll & 0x7F)) / MagicConstantScale);
+	}
+	else {
+		ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 + pll) / MagicConstantScale);
+	}
+	/*
+	    In above deprecated FP version, worst case arguments can produce a result that overflows a signed long.
+	    Here, for such cases, we emulate the FP behavior of setting the result to the maximum positive value, as
+	    the compiler's conversion of a u64 to an s32 is simple truncation of the u64's high half, sadly....
+	*/
+	if  (ResultLL > 0x7FFFFFFF)
+		gyro_sf = 0x7FFFFFFF;
+	else
+		gyro_sf = (long)ResultLL;
+
+	pdev->_gyroSF = gyro_sf; // Record value so we can debug print it
+
+	// Finally, write the value to the DMP GYRO_SF register
+	unsigned char gyro_sf_reg[4];
+	gyro_sf_reg[0] = (unsigned char)(gyro_sf >> 24);
+	gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
+	gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
+	gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
+	result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
+
+	return result;
+}
diff --git a/src/util/ICM_20948_C.h b/src/util/ICM_20948_C.h
index 3929451..94ea0da 100644
--- a/src/util/ICM_20948_C.h
+++ b/src/util/ICM_20948_C.h
@@ -54,6 +54,7 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
 		ICM_20948_Stat_DMPNotSupported, // DMP not supported (no #define ICM_20948_USE_DMP)
 		ICM_20948_Stat_DMPVerifyFail, // DMP was written but did not verify correctly
 		ICM_20948_Stat_FIFONoDataAvail, // FIFO contains no data
+		ICM_20948_Stat_FIFOIncompleteData, // FIFO contained incomplete data
 		ICM_20948_Stat_FIFOMoreDataAvail, // FIFO contains more data
 		ICM_20948_Stat_UnrecognisedDMPHeader,
 		ICM_20948_Stat_UnrecognisedDMPHeader2,
@@ -161,6 +162,12 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
 		bool _firmware_loaded; // Indicates if DMP has been loaded
 		uint8_t _last_bank; // Keep track of which bank was selected last - to avoid unnecessary writes
 		uint8_t _last_mems_bank; // Keep track of which bank was selected last - to avoid unnecessary writes
+		int32_t _gyroSF; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
+		int8_t _gyroSFpll;
+		uint32_t _enabled_Android_0; // Keep track of which Android sensors are enabled: 0-31
+		uint32_t _enabled_Android_1; // Keep track of which Android sensors are enabled: 32-
+		uint32_t _enabled_Android_intr_0; // Keep track of which Android sensor interrupts are enabled: 0-31
+		uint32_t _enabled_Android_intr_1; // Keep track of which Android sensor interrupts are enabled: 32-
 	} ICM_20948_Device_t;				 // Definition of device struct type
 
 	/*
@@ -268,6 +275,7 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
 	enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor);
 
 	ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_20948_DMP_data_t *data);
+	ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level);
 
 	// ToDo:
 
diff --git a/src/util/ICM_20948_DMP.h b/src/util/ICM_20948_DMP.h
index 2d04399..5ed733d 100644
--- a/src/util/ICM_20948_DMP.h
+++ b/src/util/ICM_20948_DMP.h
@@ -116,7 +116,7 @@ extern "C" {
 #define ACCEL_ACCURACY			(97 * 16)
 #define ACCEL_CAL_RESET			(77 * 16)
 #define ACCEL_VARIANCE_THRESH	(93 * 16)
-#define ACCEL_CAL_RATE			(94 * 16 + 4) // 16-bit
+#define ACCEL_CAL_RATE			(94 * 16 + 4) // 16-bit: 0 (225Hz, 112Hz, 56Hz)
 #define ACCEL_PRE_SENSOR_DATA	(97 * 16 + 4)
 #define ACCEL_COVARIANCE		(101 * 16 + 8)
 #define ACCEL_ALPHA_VAR			(91 * 16) // 32-bit: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
@@ -327,6 +327,7 @@ enum inv_icm20948_sensor {
 	INV_ICM20948_SENSOR_LINEAR_ACCELERATION,
 	INV_ICM20948_SENSOR_ORIENTATION,
 	INV_ICM20948_SENSOR_B2S,
+	INV_ICM20948_SENSOR_RAW_MAGNETOMETER,
 	INV_ICM20948_SENSOR_MAX,
 };