Starting to read FIFO data

Author: PaulZC

examples/Arduino/Example5_DMP/Example5_DMP.ino

@@ -10,12 +10,14 @@
  * ** This example is based on the InvenSense Application Note "Programming Sequence for DMP Hardware Functions".
  * ** We are grateful to InvenSense for providing this.
  * 
- * ** Important note: by default the DMP functionality is disabled in the library. This is to save program memory.
- * ** The DMP firmware takes up 14290 Bytes of program memory. To use the DMP, you will need to:
+ * ** Important note: by default the DMP functionality is disabled in the library
+ * ** as the DMP firmware takes up 14290 Bytes of program memory.
+ * ** To use the DMP, you will need to:
  * ** Edit ICM_20948_C.h
  * ** Uncomment line 29: #define ICM_20948_USE_DMP
  * ** Save changes
- * ** If you are using Windows, you can find ICM_20948_C.h in Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
+ * ** 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.
  *
@@ -169,21 +171,18 @@ void setup() {
   // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
   // Magnetometer full scale is +/- 4900uT so _I think_ we need to multiply by 2^30 / 4900 = 0x000357FA
   // The magnetometer Y and Z axes are reversed compared to the accelerometer so we'll invert those
-//  const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
-//  const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
-//  const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
   const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
-  const unsigned char mountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00};
-  const unsigned char mountMultiplierMinus[4] = {0xC0, 0x00, 0x00, 0x00};
+  const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
+  const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
   success &= (myICM.writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
   success &= (myICM.writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
   success &= (myICM.writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
   success &= (myICM.writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
-  success &= (myICM.writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_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, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
 
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
@@ -219,20 +218,32 @@ void setup() {
 
 void loop()
 {
-  uint16_t count;
-  myICM.getFIFOcount(&count);
+  icm_20948_DMP_data_t data;
+  myICM.readDMPdataFromFIFO(&data);
   if( myICM.status == ICM_20948_Stat_Ok )
   {
-    SERIAL_PORT.print("FIFO count is: ");
-    SERIAL_PORT.println( count );
+    SERIAL_PORT.print("Received data! Header: ");
+    SERIAL_PORT.println( data.header );
+    if ( data.header == DMP_header_bitmap_Quat9 ) // We have asked for orientation data so we should receive Quat9
+    {
+      SERIAL_PORT.print("Quat9 data is: 0x");
+      for (int i = 0; i < 14; i++) // Quat9 data is 14 bytes long
+      {
+        if ( data.Quat9[i] < 16) SERIAL_PORT.print( "0" ); // Pad the zero
+        SERIAL_PORT.print( data.Quat9[i], HEX );
+      }
+      SERIAL_PORT.println();
+    }
   }
-  else
+  else if ( myICM.status != ICM_20948_Stat_FIFONoDataAvail )
   {
-    SERIAL_PORT.print("getFIFOcount failed! Status is: ");
+    SERIAL_PORT.print("readDMPdataFromFIFO failed! Status is: ");
     SERIAL_PORT.println( myICM.statusString() );
+    SERIAL_PORT.print("Header is: 0x");
+    SERIAL_PORT.println( data.header, HEX );
   }
 
-  delay(1000);
+  delay(10);
 }
 
 

src/ICM_20948.cpp

@@ -953,7 +953,7 @@ ICM_20948_Status_e ICM_20948::enableDMP(bool enable)
 {
     if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP?
     {
-        status = ICM_20948_enable_DMP(&_device, enable);
+        status = ICM_20948_enable_DMP(&_device, enable == true ? 1 : 0 );
         return status;
     }
     return ICM_20948_Stat_DMPNotSupported;
@@ -1029,13 +1029,28 @@ ICM_20948_Status_e ICM_20948::setDMPODRrate(enum inv_icm20948_sensor sensor, int
 {
   if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR?
   {
+    // 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:
+  	// Value = (DMP running rate (225Hz) / ODR ) - 1
+  	// E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8.
+
     uint16_t period = (225 / rate) - 1;
     status = inv_icm20948_set_dmp_sensor_period(&_device, sensor, period);
     return status;
   }
   return ICM_20948_Stat_DMPNotSupported;
 }
 
+ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data)
+{
+  if (_device._dmp_firmware_available == true) // Should we attempt to set the data from the FIFO?
+  {
+    status = inv_icm20948_read_dmp_data(&_device, data);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
+}
+
 // I2C
 ICM_20948_I2C::ICM_20948_I2C()
 {

src/ICM_20948.h

@@ -119,7 +119,7 @@ class ICM_20948
     ICM_20948_Status_e cfgIntLatch(bool latching);         // If not latching then the interrupt is a 50 us pulse
     ICM_20948_Status_e cfgIntAnyReadToClear(bool enabled); // If enabled, *ANY* read will clear the INT_STATUS register. So if you have multiple interrupt sources enabled be sure to read INT_STATUS first
     ICM_20948_Status_e cfgFsyncActiveLow(bool active_low);
-    ICM_20948_Status_e cfgFsyncIntMode(bool interrupt_mode); // Can ue FSYNC as an interrupt input that sets the I2C Master Status register's PASS_THROUGH bit
+    ICM_20948_Status_e cfgFsyncIntMode(bool interrupt_mode); // Can use FSYNC as an interrupt input that sets the I2C Master Status register's PASS_THROUGH bit
 
     ICM_20948_Status_e intEnableI2C(bool enable);
     ICM_20948_Status_e intEnableDMP(bool enable);
@@ -171,6 +171,33 @@ class ICM_20948
     ICM_20948_Status_e readFIFO(uint8_t *data);
 
     //DMP
+
+    // Done:
+    //  Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+    //  Load Firmware
+    //  Configure Accel scaling to DMP
+    //  Configure Compass mount matrix and scale to DMP
+    //  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 at multiple ODRs
+
+    // 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
+    //  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
+
     ICM_20948_Status_e enableDMP(bool enable = true);
     ICM_20948_Status_e resetDMP(void);
     ICM_20948_Status_e loadDMPFirmware(void);
@@ -180,6 +207,9 @@ class ICM_20948
     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 inv_icm20948_sensor sensor, int rate);
+
+    ICM_20948_Status_e readDMPdataFromFIFO(icm_20948_DMP_data_t *data);
+
 };
 
 // I2C