Corrections and extra debug for issue #8

Author: PaulZC

examples/Arduino/Example2_Advanced/Example2_Advanced.ino

@@ -98,8 +98,6 @@ void setup()
     SERIAL_PORT.println(myICM.statusString());
   }
 
-  delay(1); // Give the ICM20948 time to change the sample mode (see issue #8)
-
   // Set full scale ranges for both acc and gyr
   ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
 

src/ICM_20948.cpp

@@ -395,6 +395,7 @@ bool ICM_20948::isConnected(void)
 ICM_20948_Status_e ICM_20948::setSampleMode(uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode)
 {
   status = ICM_20948_set_sample_mode(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode);
+  delay(1); // Give the ICM20948 time to change the sample mode (see issue #8)
   return status;
 }
 

src/util/ICM_20948_C.c

@@ -627,6 +627,26 @@ ICM_20948_Status_e ICM_20948_set_sample_mode(ICM_20948_Device_t *pdev, ICM_20948
   {
     return retval;
   }
+
+  // Check the data was written correctly
+  retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
+  if (retval != ICM_20948_Stat_Ok)
+  {
+    return retval;
+  }
+  if (sensors & ICM_20948_Internal_Acc)
+  {
+    if (reg.ACCEL_CYCLE != mode) retval = ICM_20948_Stat_Err;
+  }
+  if (sensors & ICM_20948_Internal_Gyr)
+  {
+    if (reg.GYRO_CYCLE != mode) retval = ICM_20948_Stat_Err;
+  }
+  if (sensors & ICM_20948_Internal_Mst)
+  {
+    if (reg.I2C_MST_CYCLE != mode) retval = ICM_20948_Stat_Err;
+  }
+
   return retval;
 }
 
@@ -646,6 +666,9 @@ ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_
     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
     reg.ACCEL_FS_SEL = fss.a;
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    if (reg.ACCEL_FS_SEL != fss.a) retval |= ICM_20948_Stat_Err;
   }
   if (sensors & ICM_20948_Internal_Gyr)
   {
@@ -654,6 +677,9 @@ ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_
     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
     reg.GYRO_FS_SEL = fss.g;
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    if (reg.GYRO_FS_SEL != fss.g) retval |= ICM_20948_Stat_Err;
   }
   return retval;
 }
@@ -674,6 +700,9 @@ ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_In
     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
     reg.ACCEL_DLPFCFG = cfg.a;
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    if (reg.ACCEL_DLPFCFG != cfg.a) retval |= ICM_20948_Stat_Err;
   }
   if (sensors & ICM_20948_Internal_Gyr)
   {
@@ -682,6 +711,9 @@ ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_In
     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
     reg.GYRO_DLPFCFG = cfg.g;
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    if (reg.GYRO_DLPFCFG != cfg.g) retval |= ICM_20948_Stat_Err;
   }
   return retval;
 }
@@ -709,6 +741,16 @@ ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_Int
       reg.ACCEL_FCHOICE = 0;
     }
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+    if (enable)
+    {
+      if (reg.ACCEL_FCHOICE != 1) retval |= ICM_20948_Stat_Err;
+    }
+    else
+    {
+      if (reg.ACCEL_FCHOICE != 0) retval |= ICM_20948_Stat_Err;
+    }
   }
   if (sensors & ICM_20948_Internal_Gyr)
   {
@@ -724,6 +766,16 @@ ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_Int
       reg.GYRO_FCHOICE = 0;
     }
     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    // Check the data was written correctly
+    retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+    if (enable)
+    {
+      if (reg.GYRO_FCHOICE != 1) retval |= ICM_20948_Stat_Err;
+    }
+    else
+    {
+      if (reg.GYRO_FCHOICE != 0) retval |= ICM_20948_Stat_Err;
+    }
   }
   return retval;
 }