Don't place accel and gyro into low power mode. Ensure gyro DLPF is e…

…nabled. Return chip to low power state changing DMP (even though accel and gyro are _not_ in low power mode).

src/ICM_20948.cpp

@@ -1215,8 +1215,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   uint8_t pwrMgmt2 = 0x40;                                                          // Set the reserved bit 6 (pressure sensor disable?)
   result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register
 
-  // Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
-  result = setSampleMode((ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result;
+  // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG
+  // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode
+  result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result;
 
   // Disable the FIFO
   result = enableFIFO(false); if (result > worstResult) worstResult = result;
@@ -1239,6 +1240,11 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
                          // dps2000
   result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result;
 
+  // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB))
+  // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte...
+  // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway
+  result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result;
+
   // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
   // If we see this interrupt, we'll need to reset the FIFO
   //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs
@@ -1262,7 +1268,10 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   ICM_20948_smplrt_t mySmplrt;
   mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13).
   mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13).
-  // ** Note: comment the next line to leave the sample rates at the maximum **
+  //mySmplrt.g = 4; // 225Hz
+  //mySmplrt.a = 4; // 225Hz
+  //mySmplrt.g = 8; // 112Hz
+  //mySmplrt.a = 8; // 112Hz
   result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result;
 
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
@@ -1345,17 +1354,20 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
 
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
   const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
-  //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz
+  //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz
+  //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz
   result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result;
 
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
   const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
-  //const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  //const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz
+  //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz
   result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result;
 
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
   const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
-  //const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  //const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz
+  //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz
   result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result;
 
   // Configure the Accel Cal Rate

src/util/ICM_20948_C.c

@@ -1297,10 +1297,9 @@ ICM_20948_Status_e inv_icm20948_firmware_load(ICM_20948_Device_t *pdev, const un
   }
 
   //Enable LP_EN since we disabled it at begining of this function.
-
-  // result = ICM_20948_low_power(pdev, true); // Put chip into low power state
-  // if (result != ICM_20948_Stat_Ok)
-  // 		return result;
+  result = ICM_20948_low_power(pdev, true); // Put chip into low power state
+  if (result != ICM_20948_Stat_Ok)
+    return result;
 
   if (!flag)
   {
@@ -1602,9 +1601,9 @@ ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev,
     break;
   }
 
-  // result = ICM_20948_low_power(pdev, true); // Put chip into low power state
-  // if (result != ICM_20948_Stat_Ok)
-  // 		return result;
+  result = ICM_20948_low_power(pdev, true); // Put chip into low power state
+  if (result != ICM_20948_Stat_Ok)
+    return result;
 
   if (result2 > result)
     result = result2; // Return the highest error
@@ -1774,9 +1773,9 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
     return result;
   }
 
-  // result = ICM_20948_low_power(pdev, true); // Put chip into low power state
-  // if (result != ICM_20948_Stat_Ok)
-  // 		return result;
+  result = ICM_20948_low_power(pdev, true); // Put chip into low power state
+  if (result != ICM_20948_Stat_Ok)
+    return result;
 
   return result;
 }
@@ -1861,9 +1860,9 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev,
   // 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);
 
-  // result = ICM_20948_low_power(pdev, true); // Put chip into low power state
-  // if (result != ICM_20948_Stat_Ok)
-  // 		return result;
+  result = ICM_20948_low_power(pdev, true); // Put chip into low power state
+  if (result != ICM_20948_Stat_Ok)
+    return result;
 
   return result;
 }