Adding readFIFO

Author: PaulZC

src/ICM_20948.cpp

@@ -946,6 +946,12 @@ ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count)
   return status;
 }
 
+ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data)
+{
+  status = ICM_20948_read_FIFO(&_device, data);
+  return status;
+}
+
 // DMP
 
 ICM_20948_Status_e ICM_20948::enableDMP(bool enable)

src/ICM_20948.h

@@ -168,6 +168,7 @@ class ICM_20948
     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);
+    ICM_20948_Status_e readFIFO(uint8_t *data);
 
     //DMP
     ICM_20948_Status_e enableDMP(bool enable = true);

src/util/ICM_20948_C.c

@@ -992,6 +992,28 @@ ICM_20948_Status_e ICM_20948_get_FIFO_count(ICM_20948_Device_t *pdev, uint16_t *
 	return retval;
 }
 
+ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t *pdev, uint8_t *data)
+{
+	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
+
+	ICM_20948_FIFO_R_W_t ctrl;
+	retval = ICM_20948_set_bank(pdev, 0);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_R_W, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_R_W_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+
+	*data = ctrl.FIFO_R_W;
+
+	return retval;
+}
+
 // DMP
 
 ICM_20948_Status_e ICM_20948_enable_DMP(ICM_20948_Device_t *pdev, bool enable)

src/util/ICM_20948_C.h

@@ -247,6 +247,7 @@ callbacks for the user to respond to interrupt events
 	ICM_20948_Status_e ICM_20948_reset_FIFO(ICM_20948_Device_t *pdev);
 	ICM_20948_Status_e ICM_20948_set_FIFO_mode(ICM_20948_Device_t *pdev, bool snapshot);
 	ICM_20948_Status_e ICM_20948_get_FIFO_count(ICM_20948_Device_t *pdev, uint16_t *count);
+	ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t *pdev, uint8_t *data);
 
 	// DMP