Extra diagnostics

Author: PaulZC

src/ICM_20948.cpp

@@ -1047,6 +1047,12 @@ ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, b
     debugPrintf((int)_device._enabled_Android_0);
     debugPrint(F("  _enabled_Android_1: "));
     debugPrintf((int)_device._enabled_Android_1);
+    debugPrint(F("  _dataOutCtl1: "));
+    debugPrintf((int)_device._dataOutCtl1);
+    debugPrint(F("  _dataOutCtl2: "));
+    debugPrintf((int)_device._dataOutCtl2);
+    debugPrint(F("  _dataRdyStatus: "));
+    debugPrintf((int)_device._dataRdyStatus);
     debugPrintln(F(""));
     return status;
   }
@@ -1062,6 +1068,8 @@ ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor
     debugPrintf((int)_device._enabled_Android_intr_0);
     debugPrint(F("  _enabled_Android_intr_1: "));
     debugPrintf((int)_device._enabled_Android_intr_1);
+    debugPrint(F("  _dataIntrCtl: "));
+    debugPrintf((int)_device._dataIntrCtl);
     debugPrintln(F(""));
     return status;
   }

src/util/ICM_20948_C.c

@@ -1723,6 +1723,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
   unsigned char data_output_control_reg[2];
   data_output_control_reg[0] = (unsigned char)(delta >> 8);
   data_output_control_reg[1] = (unsigned char)(delta & 0xff);
+  pdev->_dataOutCtl1 = delta; // Diagnostics
   result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL1, 2, (const unsigned char *)&data_output_control_reg);
   if (result != ICM_20948_Stat_Ok)
   {
@@ -1732,6 +1733,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
   // Write the 'header2' sensor control bits into memory address DATA_OUT_CTL2
   data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
   data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
+  pdev->_dataOutCtl2 = delta2; // Diagnostics
   result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL2, 2, (const unsigned char *)&data_output_control_reg);
   if (result != ICM_20948_Stat_Ok)
   {
@@ -1741,6 +1743,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
   // 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);
+  pdev->_dataRdyStatus = data_rdy_status; // Diagnostics
   result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
   if (result != ICM_20948_Stat_Ok)
   {
@@ -1764,6 +1767,7 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
   // 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);
+  pdev->_motionEventCtl = inv_event_control; // Diagnostics
   result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
   if (result != ICM_20948_Stat_Ok)
   {
@@ -1836,11 +1840,6 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev,
     }
   }
 
-  unsigned char data_intr_ctl[2];
-
-  data_intr_ctl[0] = (unsigned char)(delta >> 8);
-  data_intr_ctl[1] = (unsigned char)(delta & 0xff);
-
   result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
   if (result != ICM_20948_Stat_Ok)
   {
@@ -1853,6 +1852,12 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev,
     return result;
   }
 
+  unsigned char data_intr_ctl[2];
+
+  data_intr_ctl[0] = (unsigned char)(delta >> 8);
+  data_intr_ctl[1] = (unsigned char)(delta & 0xff);
+  pdev->_dataIntrCtl = delta; // Diagnostics
+  
   // 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);
 

src/util/ICM_20948_C.h

@@ -172,6 +172,11 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
     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-
+    uint16_t _dataOutCtl1;            // Diagnostics: record the setting of DATA_OUT_CTL1
+    uint16_t _dataOutCtl2;            // Diagnostics: record the setting of DATA_OUT_CTL2
+    uint16_t _dataRdyStatus;          // Diagnostics: record the setting of DATA_RDY_STATUS
+    uint16_t _motionEventCtl;         // Diagnostics: record the setting of MOTION_EVENT_CTL
+    uint16_t _dataIntrCtl;            // Diagnostics: record the setting of DATA_INTR_CTL
   } ICM_20948_Device_t;               // Definition of device struct type
 
   // ICM_20948_Status_e ICM_20948_Startup( ICM_20948_Device_t* pdev ); // For the time being this performs a standardized startup routine