Adding FIFO and DMP functionality

Author: PaulZC

examples/Arduino/Example5_DMP/Example5_DMP.ino

@@ -13,7 +13,7 @@
  ***************************************************************/
 #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 USE_SPI       // Uncomment this to use SPI
 
 #define SERIAL_PORT Serial
 
@@ -34,8 +34,18 @@
 
 void setup() {
 
-  SERIAL_PORT.begin(115200);
-  while(!SERIAL_PORT){};
+  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();
@@ -67,19 +77,53 @@ void setup() {
 
   SERIAL_PORT.println("Device connected!");
 
+  myICM.enableDMP(); // Enable the DMP
+
+  if( myICM.status == ICM_20948_Stat_Ok )
+    SERIAL_PORT.println("DMP enabled!");
+  else
+  {
+    SERIAL_PORT.print("Enable DMP failed! Status is: ");
+    SERIAL_PORT.println( myICM.statusString() );
+  }
+  
+  myICM.enableFIFO(); // Enable the FIFO
+
+  if( myICM.status == ICM_20948_Stat_Ok )
+    SERIAL_PORT.println("FIFO enabled!");
+  else
+  {
+    SERIAL_PORT.print("Enable FIFO failed! Status is: ");
+    SERIAL_PORT.println( myICM.statusString() );
+  }
+  
   myICM.enableSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR); // Enable the Game Rotation Vector
 
   if( myICM.status == ICM_20948_Stat_Ok )
     SERIAL_PORT.println("INV_ICM20948_SENSOR_ROTATION_VECTOR enabled!");
   else
   {
-    SERIAL_PORT.println("INV_ICM20948_SENSOR_ROTATION_VECTOR failed! Status is: ");
+    SERIAL_PORT.print("INV_ICM20948_SENSOR_ROTATION_VECTOR failed! Status is: ");
     SERIAL_PORT.println( myICM.statusString() );
   }
 }
 
-void loop() {
+void loop()
+{
+  uint16_t count;
+  myICM.getFIFOcount(&count);
+  if( myICM.status == ICM_20948_Stat_Ok )
+  {
+    SERIAL_PORT.print("FIFO count is: ");
+    SERIAL_PORT.println( count );
+  }
+  else
+  {
+    SERIAL_PORT.print("getFIFOcount failed! Status is: ");
+    SERIAL_PORT.println( myICM.statusString() );
+  }
 
+  delay(1000);
 }
 
 

src/ICM_20948.cpp

@@ -919,8 +919,47 @@ ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
     return status;
 }
 
+// FIFO
+
+ICM_20948_Status_e ICM_20948::enableFIFO(bool enable)
+{
+    status = ICM_20948_enable_FIFO(&_device, enable);
+    return status;
+}
+
+ICM_20948_Status_e ICM_20948::resetFIFO(void)
+{
+  status = ICM_20948_reset_FIFO(&_device);
+  return status;
+}
+
+ICM_20948_Status_e ICM_20948::setFIFOmode(bool snapshot)
+{
+    // Default to Stream (non-Snapshot) mode
+    status = ICM_20948_set_FIFO_mode(&_device, snapshot);
+    return status;
+}
+
+ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count)
+{
+  status = ICM_20948_get_FIFO_count(&_device, count);
+  return status;
+}
+
 // DMP
 
+ICM_20948_Status_e ICM_20948::enableDMP(bool enable)
+{
+    status = ICM_20948_enable_DMP(&_device, enable);
+    return status;
+}
+
+ICM_20948_Status_e ICM_20948::resetDMP(void)
+{
+    status = ICM_20948_reset_DMP(&_device);
+    return status;
+}
+
 ICM_20948_Status_e ICM_20948::loadDMPFirmware(void)
 {
     status = ICM_20948_firmware_load(&_device);

src/ICM_20948.h

@@ -163,7 +163,15 @@ class ICM_20948
     uint8_t readMag(AK09916_Reg_Addr_e reg);
     ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata);
 
+    //FIFO
+    ICM_20948_Status_e enableFIFO(bool enable = true);
+    ICM_20948_Status_e resetFIFO(void);
+    ICM_20948_Status_e setFIFOmode(bool snapshot = false); // Default to Stream (non-Snapshot) mode
+    ICM_20948_Status_e getFIFOcount(uint16_t *count);
+
     //DMP
+    ICM_20948_Status_e enableDMP(bool enable = true);
+    ICM_20948_Status_e resetDMP(void);
     ICM_20948_Status_e loadDMPFirmware(void);
     ICM_20948_Status_e enableSensor(enum inv_icm20948_sensor sensor, bool enable = true);
 };