ICM_20948_C.c

00001 #include "ICM_20948_C.h"
00002 #include "ICM_20948_REGISTERS.h"
00003 #include "AK09916_REGISTERS.h"
00004 
00005 /*
00006  * Icm20948 device require a DMP image to be loaded on init
00007  * Provide such images by mean of a byte array
00008 */
00009 #if defined(ICM_20948_USE_DMP) // Only include the 14301 Bytes of DMP if ICM_20948_USE_DMP is defined
00010 
00011 #if defined(ARDUINO_ARCH_MBED) // ARDUINO_ARCH_MBED (APOLLO3 v2) does not support or require pgmspace.h / PROGMEM
00012 const uint8_t dmp3_image[] = {
00013 #elif (defined(__AVR__) || defined(__arm__) || defined(__ARDUINO_ARC__)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
00014 #define ICM_20948_USE_PROGMEM_FOR_DMP
00015 #include <avr/pgmspace.h>
00016 const uint8_t dmp3_image[] PROGMEM = {
00017 #else
00018 const uint8_t dmp3_image[] = {
00019 #endif
00020 
00021 #include "icm20948_img.dmp3a.h"
00022 };
00023 #endif
00024 
00025 // ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t
00026 const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] =
00027     {
00028         3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8, 13, 12 // Also used for Geomag
00029 };
00030 const int DMP_Quat6_Byte_Ordering[icm_20948_DMP_Quat6_Bytes] =
00031     {
00032         3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8 // Also used for Gyro_Calibr, Compass_Calibr
00033 };
00034 const int DMP_PQuat6_Byte_Ordering[icm_20948_DMP_PQuat6_Bytes] =
00035     {
00036         1, 0, 3, 2, 5, 4 // Also used for Raw_Accel, Compass
00037 };
00038 const int DMP_Raw_Gyro_Byte_Ordering[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes] =
00039     {
00040         1, 0, 3, 2, 5, 4, 7, 6, 9, 8, 11, 10};
00041 const int DMP_Activity_Recognition_Byte_Ordering[icm_20948_DMP_Activity_Recognition_Bytes] =
00042     {
00043         0, 1, 5, 4, 3, 2};
00044 const int DMP_Secondary_On_Off_Byte_Ordering[icm_20948_DMP_Secondary_On_Off_Bytes] =
00045     {
00046         1, 0};
00047 
00048 const uint16_t inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX] =
00049     {
00050         // Data output control 1 register bit definition
00051         // 16-bit accel                                0x8000
00052         // 16-bit gyro                                 0x4000
00053         // 16-bit compass                              0x2000
00054         // 16-bit ALS                                  0x1000
00055         // 32-bit 6-axis quaternion                    0x0800
00056         // 32-bit 9-axis quaternion + heading accuracy 0x0400
00057         // 16-bit pedometer quaternion                 0x0200
00058         // 32-bit Geomag rv + heading accuracy         0x0100
00059         // 16-bit Pressure                             0x0080
00060         // 32-bit calibrated gyro                      0x0040
00061         // 32-bit calibrated compass                   0x0020
00062         // Pedometer Step Detector                     0x0010
00063         // Header 2                                    0x0008
00064         // Pedometer Step Indicator Bit 2              0x0004
00065         // Pedometer Step Indicator Bit 1              0x0002
00066         // Pedometer Step Indicator Bit 0              0x0001
00067         // Unsupported Sensors are 0xFFFF
00068 
00069         0xFFFF, // 0  Meta Data
00070         0x8008, // 1  Accelerometer
00071         0x0028, // 2  Magnetic Field
00072         0x0408, // 3  Orientation
00073         0x4048, // 4  Gyroscope
00074         0x1008, // 5  Light
00075         0x0088, // 6  Pressure
00076         0xFFFF, // 7  Temperature
00077         0xFFFF, // 8  Proximity <----------- fixme
00078         0x0808, // 9  Gravity
00079         0x8808, // 10 Linear Acceleration
00080         0x0408, // 11 Rotation Vector
00081         0xFFFF, // 12 Humidity
00082         0xFFFF, // 13 Ambient Temperature
00083         0x2008, // 14 Magnetic Field Uncalibrated
00084         0x0808, // 15 Game Rotation Vector
00085         0x4008, // 16 Gyroscope Uncalibrated
00086         0x0000, // 17 Significant Motion
00087         0x0018, // 18 Step Detector
00088         0x0010, // 19 Step Counter <----------- fixme
00089         0x0108, // 20 Geomagnetic Rotation Vector
00090         0xFFFF, // 21 ANDROID_SENSOR_HEART_RATE,
00091         0xFFFF, // 22 ANDROID_SENSOR_PROXIMITY,
00092 
00093         0x8008, // 23 ANDROID_SENSOR_WAKEUP_ACCELEROMETER,
00094         0x0028, // 24 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD,
00095         0x0408, // 25 ANDROID_SENSOR_WAKEUP_ORIENTATION,
00096         0x4048, // 26 ANDROID_SENSOR_WAKEUP_GYROSCOPE,
00097         0x1008, // 27 ANDROID_SENSOR_WAKEUP_LIGHT,
00098         0x0088, // 28 ANDROID_SENSOR_WAKEUP_PRESSURE,
00099         0x0808, // 29 ANDROID_SENSOR_WAKEUP_GRAVITY,
00100         0x8808, // 30 ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION,
00101         0x0408, // 31 ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR,
00102         0xFFFF, // 32 ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY,
00103         0xFFFF, // 33 ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE,
00104         0x2008, // 34 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED,
00105         0x0808, // 35 ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR,
00106         0x4008, // 36 ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED,
00107         0x0018, // 37 ANDROID_SENSOR_WAKEUP_STEP_DETECTOR,
00108         0x0010, // 38 ANDROID_SENSOR_WAKEUP_STEP_COUNTER,
00109         0x0108, // 39 ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR
00110         0xFFFF, // 40 ANDROID_SENSOR_WAKEUP_HEART_RATE,
00111         0x0000, // 41 ANDROID_SENSOR_WAKEUP_TILT_DETECTOR,
00112         0x8008, // 42 Raw Acc
00113         0x4048, // 43 Raw Gyr
00114 };
00115 
00116 const ICM_20948_Serif_t NullSerif = {
00117     NULL, // write
00118     NULL, // read
00119     NULL, // user
00120 };
00121 
00122 // Private function prototypes
00123 
00124 // Function definitions
00125 ICM_20948_Status_e ICM_20948_init_struct(ICM_20948_Device_t *pdev)
00126 {
00127   // Initialize all elements by 0 except for _last_bank
00128   // Initialize _last_bank to 4 (invalid bank number)
00129   // so ICM_20948_set_bank function does not skip issuing bank change operation
00130   static const ICM_20948_Device_t init_device = { ._last_bank = 4 };
00131   *pdev = init_device;
00132   return ICM_20948_Stat_Ok;
00133 }
00134 
00135 ICM_20948_Status_e ICM_20948_link_serif(ICM_20948_Device_t *pdev, const ICM_20948_Serif_t *s)
00136 {
00137   if (s == NULL)
00138   {
00139     return ICM_20948_Stat_ParamErr;
00140   }
00141   if (pdev == NULL)
00142   {
00143     return ICM_20948_Stat_ParamErr;
00144   }
00145   pdev->_serif = s;
00146   return ICM_20948_Stat_Ok;
00147 }
00148 
00149 ICM_20948_Status_e ICM_20948_execute_w(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len)
00150 {
00151   if (pdev->_serif->write == NULL)
00152   {
00153     return ICM_20948_Stat_NotImpl;
00154   }
00155   return (*pdev->_serif->write)(regaddr, pdata, len, pdev->_serif->user);
00156 }
00157 
00158 ICM_20948_Status_e ICM_20948_execute_r(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len)
00159 {
00160   if (pdev->_serif->read == NULL)
00161   {
00162     return ICM_20948_Stat_NotImpl;
00163   }
00164   return (*pdev->_serif->read)(regaddr, pdata, len, pdev->_serif->user);
00165 }
00166 
00167 //Transact directly with an I2C device, one byte at a time
00168 //Used to configure a device before it is setup into a normal 0-3 peripheral slot
00169 ICM_20948_Status_e ICM_20948_i2c_controller_periph4_txn(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr)
00170 {
00171   // Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86
00172   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00173 
00174   addr = (((Rw) ? 0x80 : 0x00) | addr);
00175 
00176   retval = ICM_20948_set_bank(pdev, 3);
00177   retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_ADDR, (uint8_t *)&addr, 1);
00178   if (retval != ICM_20948_Stat_Ok)
00179   {
00180     return retval;
00181   }
00182 
00183   retval = ICM_20948_set_bank(pdev, 3);
00184   retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_REG, (uint8_t *)&reg, 1);
00185   if (retval != ICM_20948_Stat_Ok)
00186   {
00187     return retval;
00188   }
00189 
00190   ICM_20948_I2C_PERIPH4_CTRL_t ctrl;
00191   ctrl.EN = 1;
00192   ctrl.INT_EN = false;
00193   ctrl.DLY = 0;
00194   ctrl.REG_DIS = !send_reg_addr;
00195 
00196   ICM_20948_I2C_MST_STATUS_t i2c_mst_status;
00197   bool txn_failed = false;
00198   uint16_t nByte = 0;
00199 
00200   while (nByte < len)
00201   {
00202     if (!Rw)
00203     {
00204       retval = ICM_20948_set_bank(pdev, 3);
00205       retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_DO, (uint8_t *)&(data[nByte]), 1);
00206       if (retval != ICM_20948_Stat_Ok)
00207       {
00208         return retval;
00209       }
00210     }
00211 
00212     // Kick off txn
00213     retval = ICM_20948_set_bank(pdev, 3);
00214     retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_PERIPH4_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_PERIPH4_CTRL_t));
00215     if (retval != ICM_20948_Stat_Ok)
00216     {
00217       return retval;
00218     }
00219 
00220     // long tsTimeout = millis() + 3000;  // Emergency timeout for txn (hard coded to 3 secs)
00221     uint32_t max_cycles = 1000;
00222     uint32_t count = 0;
00223     bool peripheral4Done = false;
00224     while (!peripheral4Done)
00225     {
00226       retval = ICM_20948_set_bank(pdev, 0);
00227       retval = ICM_20948_execute_r(pdev, AGB0_REG_I2C_MST_STATUS, (uint8_t *)&i2c_mst_status, 1);
00228 
00229       peripheral4Done = (i2c_mst_status.I2C_PERIPH4_DONE /*| (millis() > tsTimeout) */); //Avoid forever-loops
00230       peripheral4Done |= (count >= max_cycles);
00231       count++;
00232     }
00233     txn_failed = (i2c_mst_status.I2C_PERIPH4_NACK /*| (millis() > tsTimeout) */);
00234     txn_failed |= (count >= max_cycles);
00235     if (txn_failed)
00236       break;
00237 
00238     if (Rw)
00239     {
00240       retval = ICM_20948_set_bank(pdev, 3);
00241       retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_PERIPH4_DI, &data[nByte], 1);
00242     }
00243 
00244     nByte++;
00245   }
00246 
00247   if (txn_failed)
00248   {
00249     //We often fail here if mag is stuck
00250     return ICM_20948_Stat_Err;
00251   }
00252 
00253   return retval;
00254 }
00255 
00256 ICM_20948_Status_e ICM_20948_i2c_master_single_w(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data)
00257 {
00258   return ICM_20948_i2c_controller_periph4_txn(pdev, addr, reg, data, 1, false, true);
00259 }
00260 
00261 ICM_20948_Status_e ICM_20948_i2c_master_single_r(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data)
00262 {
00263   return ICM_20948_i2c_controller_periph4_txn(pdev, addr, reg, data, 1, true, true);
00264 }
00265 
00266 ICM_20948_Status_e ICM_20948_set_bank(ICM_20948_Device_t *pdev, uint8_t bank)
00267 {
00268   if (bank > 3)
00269   {
00270     return ICM_20948_Stat_ParamErr;
00271   } // Only 4 possible banks
00272 
00273   if (bank == pdev->_last_bank) // Do we need to change bank?
00274     return ICM_20948_Stat_Ok;   // Bail if we don't need to change bank to avoid unnecessary bus traffic
00275 
00276   pdev->_last_bank = bank;   // Store the requested bank (before we bit-shift)
00277   bank = (bank << 4) & 0x30; // bits 5:4 of REG_BANK_SEL
00278   return ICM_20948_execute_w(pdev, REG_BANK_SEL, &bank, 1);
00279 }
00280 
00281 ICM_20948_Status_e ICM_20948_sw_reset(ICM_20948_Device_t *pdev)
00282 {
00283   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00284   ICM_20948_PWR_MGMT_1_t reg;
00285 
00286   ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00287 
00288   retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00289   if (retval != ICM_20948_Stat_Ok)
00290   {
00291     return retval;
00292   }
00293 
00294   reg.DEVICE_RESET = 1;
00295 
00296   retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00297   if (retval != ICM_20948_Stat_Ok)
00298   {
00299     return retval;
00300   }
00301   return retval;
00302 }
00303 
00304 ICM_20948_Status_e ICM_20948_sleep(ICM_20948_Device_t *pdev, bool on)
00305 {
00306   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00307   ICM_20948_PWR_MGMT_1_t reg;
00308 
00309   ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00310 
00311   retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00312   if (retval != ICM_20948_Stat_Ok)
00313   {
00314     return retval;
00315   }
00316 
00317   if (on)
00318   {
00319     reg.SLEEP = 1;
00320   }
00321   else
00322   {
00323     reg.SLEEP = 0;
00324   }
00325 
00326   retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00327   if (retval != ICM_20948_Stat_Ok)
00328   {
00329     return retval;
00330   }
00331   return retval;
00332 }
00333 
00334 ICM_20948_Status_e ICM_20948_low_power(ICM_20948_Device_t *pdev, bool on)
00335 {
00336   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00337   ICM_20948_PWR_MGMT_1_t reg;
00338 
00339   ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00340 
00341   retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00342   if (retval != ICM_20948_Stat_Ok)
00343   {
00344     return retval;
00345   }
00346 
00347   if (on)
00348   {
00349     reg.LP_EN = 1;
00350   }
00351   else
00352   {
00353     reg.LP_EN = 0;
00354   }
00355 
00356   retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00357   if (retval != ICM_20948_Stat_Ok)
00358   {
00359     return retval;
00360   }
00361   return retval;
00362 }
00363 
00364 ICM_20948_Status_e ICM_20948_set_clock_source(ICM_20948_Device_t *pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source)
00365 {
00366   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00367   ICM_20948_PWR_MGMT_1_t reg;
00368 
00369   ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00370 
00371   retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00372   if (retval != ICM_20948_Stat_Ok)
00373   {
00374     return retval;
00375   }
00376 
00377   reg.CLKSEL = source;
00378 
00379   retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
00380   if (retval != ICM_20948_Stat_Ok)
00381   {
00382     return retval;
00383   }
00384   return retval;
00385 }
00386 
00387 ICM_20948_Status_e ICM_20948_get_who_am_i(ICM_20948_Device_t *pdev, uint8_t *whoami)
00388 {
00389   if (whoami == NULL)
00390   {
00391     return ICM_20948_Stat_ParamErr;
00392   }
00393   ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00394   return ICM_20948_execute_r(pdev, AGB0_REG_WHO_AM_I, whoami, 1);
00395 }
00396 
00397 ICM_20948_Status_e ICM_20948_check_id(ICM_20948_Device_t *pdev)
00398 {
00399   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00400   uint8_t whoami = 0x00;
00401   retval = ICM_20948_get_who_am_i(pdev, &whoami);
00402   if (retval != ICM_20948_Stat_Ok)
00403   {
00404     return retval;
00405   }
00406   if (whoami != ICM_20948_WHOAMI)
00407   {
00408     return ICM_20948_Stat_WrongID;
00409   }
00410   return retval;
00411 }
00412 
00413 ICM_20948_Status_e ICM_20948_data_ready(ICM_20948_Device_t *pdev)
00414 {
00415   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00416   ICM_20948_INT_STATUS_1_t reg;
00417   retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00418   if (retval != ICM_20948_Stat_Ok)
00419   {
00420     return retval;
00421   }
00422   retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_STATUS_1, (uint8_t *)&reg, sizeof(ICM_20948_INT_STATUS_1_t));
00423   if (retval != ICM_20948_Stat_Ok)
00424   {
00425     return retval;
00426   }
00427   if (!reg.RAW_DATA_0_RDY_INT)
00428   {
00429     retval = ICM_20948_Stat_NoData;
00430   }
00431   return retval;
00432 }
00433 
00434 // Interrupt Configuration
00435 ICM_20948_Status_e ICM_20948_int_pin_cfg(ICM_20948_Device_t *pdev, ICM_20948_INT_PIN_CFG_t *write, ICM_20948_INT_PIN_CFG_t *read)
00436 {
00437   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00438   retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00439   if (write != NULL)
00440   { // write first, if available
00441     retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)write, sizeof(ICM_20948_INT_PIN_CFG_t));
00442     if (retval != ICM_20948_Stat_Ok)
00443     {
00444       return retval;
00445     }
00446   }
00447   if (read != NULL)
00448   { // then read, to allow for verification
00449     retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)read, sizeof(ICM_20948_INT_PIN_CFG_t));
00450     if (retval != ICM_20948_Stat_Ok)
00451     {
00452       return retval;
00453     }
00454   }
00455   return retval;
00456 }
00457 
00458 ICM_20948_Status_e ICM_20948_int_enable(ICM_20948_Device_t *pdev, ICM_20948_INT_enable_t *write, ICM_20948_INT_enable_t *read)
00459 {
00460   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00461 
00462   ICM_20948_INT_ENABLE_t en_0;
00463   ICM_20948_INT_ENABLE_1_t en_1;
00464   ICM_20948_INT_ENABLE_2_t en_2;
00465   ICM_20948_INT_ENABLE_3_t en_3;
00466 
00467   retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00468 
00469   if (write != NULL)
00470   { // If the write pointer is not NULL then write to the registers BEFORE reading
00471     en_0.I2C_MST_INT_EN = write->I2C_MST_INT_EN;
00472     en_0.DMP_INT1_EN = write->DMP_INT1_EN;
00473     en_0.PLL_READY_EN = write->PLL_RDY_EN;
00474     en_0.WOM_INT_EN = write->WOM_INT_EN;
00475     en_0.reserved_0 = 0; // Clear RAM garbage
00476     en_0.REG_WOF_EN = write->REG_WOF_EN;
00477     en_1.RAW_DATA_0_RDY_EN = write->RAW_DATA_0_RDY_EN;
00478     en_1.reserved_0 = 0; // Clear RAM garbage
00479     en_2.individual.FIFO_OVERFLOW_EN_4 = write->FIFO_OVERFLOW_EN_4;
00480     en_2.individual.FIFO_OVERFLOW_EN_3 = write->FIFO_OVERFLOW_EN_3;
00481     en_2.individual.FIFO_OVERFLOW_EN_2 = write->FIFO_OVERFLOW_EN_2;
00482     en_2.individual.FIFO_OVERFLOW_EN_1 = write->FIFO_OVERFLOW_EN_1;
00483     en_2.individual.FIFO_OVERFLOW_EN_0 = write->FIFO_OVERFLOW_EN_0;
00484     en_2.individual.reserved_0 = 0; // Clear RAM garbage
00485     en_3.individual.FIFO_WM_EN_4 = write->FIFO_WM_EN_4;
00486     en_3.individual.FIFO_WM_EN_3 = write->FIFO_WM_EN_3;
00487     en_3.individual.FIFO_WM_EN_2 = write->FIFO_WM_EN_2;
00488     en_3.individual.FIFO_WM_EN_1 = write->FIFO_WM_EN_1;
00489     en_3.individual.FIFO_WM_EN_0 = write->FIFO_WM_EN_0;
00490     en_3.individual.reserved_0 = 0; // Clear RAM garbage
00491 
00492     retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
00493     if (retval != ICM_20948_Stat_Ok)
00494     {
00495       return retval;
00496     }
00497     retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
00498     if (retval != ICM_20948_Stat_Ok)
00499     {
00500       return retval;
00501     }
00502     retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
00503     if (retval != ICM_20948_Stat_Ok)
00504     {
00505       return retval;
00506     }
00507     retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
00508     if (retval != ICM_20948_Stat_Ok)
00509     {
00510       return retval;
00511     }
00512   }
00513 
00514   if (read != NULL)
00515   { // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
00516     retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
00517     if (retval != ICM_20948_Stat_Ok)
00518     {
00519       return retval;
00520     }
00521     retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
00522     if (retval != ICM_20948_Stat_Ok)
00523     {
00524       return retval;
00525     }
00526     retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
00527     if (retval != ICM_20948_Stat_Ok)
00528     {
00529       return retval;
00530     }
00531     retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
00532     if (retval != ICM_20948_Stat_Ok)
00533     {
00534       return retval;
00535     }
00536 
00537     read->I2C_MST_INT_EN = en_0.I2C_MST_INT_EN;
00538     read->DMP_INT1_EN = en_0.DMP_INT1_EN;
00539     read->PLL_RDY_EN = en_0.PLL_READY_EN;
00540     read->WOM_INT_EN = en_0.WOM_INT_EN;
00541     read->REG_WOF_EN = en_0.REG_WOF_EN;
00542     read->RAW_DATA_0_RDY_EN = en_1.RAW_DATA_0_RDY_EN;
00543     read->FIFO_OVERFLOW_EN_4 = en_2.individual.FIFO_OVERFLOW_EN_4;
00544     read->FIFO_OVERFLOW_EN_3 = en_2.individual.FIFO_OVERFLOW_EN_3;
00545     read->FIFO_OVERFLOW_EN_2 = en_2.individual.FIFO_OVERFLOW_EN_2;
00546     read->FIFO_OVERFLOW_EN_1 = en_2.individual.FIFO_OVERFLOW_EN_1;
00547     read->FIFO_OVERFLOW_EN_0 = en_2.individual.FIFO_OVERFLOW_EN_0;
00548     read->FIFO_WM_EN_4 = en_3.individual.FIFO_WM_EN_4;
00549     read->FIFO_WM_EN_3 = en_3.individual.FIFO_WM_EN_3;
00550     read->FIFO_WM_EN_2 = en_3.individual.FIFO_WM_EN_2;
00551     read->FIFO_WM_EN_1 = en_3.individual.FIFO_WM_EN_1;
00552     read->FIFO_WM_EN_0 = en_3.individual.FIFO_WM_EN_0;
00553   }
00554 
00555   return retval;
00556 }
00557 
00558 ICM_20948_Status_e ICM_20948_wom_threshold(ICM_20948_Device_t *pdev, ICM_20948_ACCEL_WOM_THR_t *write, ICM_20948_ACCEL_WOM_THR_t *read)
00559 {
00560   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00561 
00562   ICM_20948_ACCEL_WOM_THR_t thr;
00563 
00564   retval = ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00565 
00566   if (write != NULL)
00567   { // If the write pointer is not NULL then write to the registers BEFORE reading
00568     thr.WOM_THRESHOLD = write->WOM_THRESHOLD;
00569 
00570     retval = ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_WOM_THR, (uint8_t *)&thr, sizeof(ICM_20948_ACCEL_WOM_THR_t));
00571     if (retval != ICM_20948_Stat_Ok)
00572     {
00573       return retval;
00574     }
00575   }
00576 
00577   if (read != NULL)
00578   { // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
00579     retval = ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_WOM_THR, (uint8_t *)&thr, sizeof(ICM_20948_ACCEL_WOM_THR_t));
00580     if (retval != ICM_20948_Stat_Ok)
00581     {
00582       return retval;
00583     }
00584 
00585     read->WOM_THRESHOLD = thr.WOM_THRESHOLD;
00586   }
00587 
00588   return retval;
00589 }
00590 
00591 ICM_20948_Status_e ICM_20948_set_sample_mode(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode)
00592 {
00593   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00594   ICM_20948_LP_CONFIG_t reg;
00595 
00596   if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst)))
00597   {
00598     return ICM_20948_Stat_SensorNotSupported;
00599   }
00600 
00601   retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
00602   if (retval != ICM_20948_Stat_Ok)
00603   {
00604     return retval;
00605   }
00606   retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
00607   if (retval != ICM_20948_Stat_Ok)
00608   {
00609     return retval;
00610   }
00611 
00612   if (sensors & ICM_20948_Internal_Acc)
00613   {
00614     reg.ACCEL_CYCLE = mode;
00615   } // Set all desired sensors to this setting
00616   if (sensors & ICM_20948_Internal_Gyr)
00617   {
00618     reg.GYRO_CYCLE = mode;
00619   }
00620   if (sensors & ICM_20948_Internal_Mst)
00621   {
00622     reg.I2C_MST_CYCLE = mode;
00623   }
00624 
00625   retval = ICM_20948_execute_w(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
00626   if (retval != ICM_20948_Stat_Ok)
00627   {
00628     return retval;
00629   }
00630 
00631   // Check the data was written correctly
00632   retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
00633   if (retval != ICM_20948_Stat_Ok)
00634   {
00635     return retval;
00636   }
00637   if (sensors & ICM_20948_Internal_Acc)
00638   {
00639     if (reg.ACCEL_CYCLE != mode) retval = ICM_20948_Stat_Err;
00640   }
00641   if (sensors & ICM_20948_Internal_Gyr)
00642   {
00643     if (reg.GYRO_CYCLE != mode) retval = ICM_20948_Stat_Err;
00644   }
00645   if (sensors & ICM_20948_Internal_Mst)
00646   {
00647     if (reg.I2C_MST_CYCLE != mode) retval = ICM_20948_Stat_Err;
00648   }
00649 
00650   return retval;
00651 }
00652 
00653 ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss)
00654 {
00655   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00656 
00657   if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
00658   {
00659     return ICM_20948_Stat_SensorNotSupported;
00660   }
00661 
00662   if (sensors & ICM_20948_Internal_Acc)
00663   {
00664     ICM_20948_ACCEL_CONFIG_t reg;
00665     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00666     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00667     reg.ACCEL_FS_SEL = fss.a;
00668     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00669     // Check the data was written correctly
00670     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00671     if (reg.ACCEL_FS_SEL != fss.a) retval |= ICM_20948_Stat_Err;
00672   }
00673   if (sensors & ICM_20948_Internal_Gyr)
00674   {
00675     ICM_20948_GYRO_CONFIG_1_t reg;
00676     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00677     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00678     reg.GYRO_FS_SEL = fss.g;
00679     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00680     // Check the data was written correctly
00681     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00682     if (reg.GYRO_FS_SEL != fss.g) retval |= ICM_20948_Stat_Err;
00683   }
00684   return retval;
00685 }
00686 
00687 ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg)
00688 {
00689   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00690 
00691   if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
00692   {
00693     return ICM_20948_Stat_SensorNotSupported;
00694   }
00695 
00696   if (sensors & ICM_20948_Internal_Acc)
00697   {
00698     ICM_20948_ACCEL_CONFIG_t reg;
00699     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00700     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00701     reg.ACCEL_DLPFCFG = cfg.a;
00702     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00703     // Check the data was written correctly
00704     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00705     if (reg.ACCEL_DLPFCFG != cfg.a) retval |= ICM_20948_Stat_Err;
00706   }
00707   if (sensors & ICM_20948_Internal_Gyr)
00708   {
00709     ICM_20948_GYRO_CONFIG_1_t reg;
00710     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00711     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00712     reg.GYRO_DLPFCFG = cfg.g;
00713     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00714     // Check the data was written correctly
00715     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00716     if (reg.GYRO_DLPFCFG != cfg.g) retval |= ICM_20948_Stat_Err;
00717   }
00718   return retval;
00719 }
00720 
00721 ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, bool enable)
00722 {
00723   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00724 
00725   if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
00726   {
00727     return ICM_20948_Stat_SensorNotSupported;
00728   }
00729 
00730   if (sensors & ICM_20948_Internal_Acc)
00731   {
00732     ICM_20948_ACCEL_CONFIG_t reg;
00733     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00734     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00735     if (enable)
00736     {
00737       reg.ACCEL_FCHOICE = 1;
00738     }
00739     else
00740     {
00741       reg.ACCEL_FCHOICE = 0;
00742     }
00743     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00744     // Check the data was written correctly
00745     retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
00746     if (enable)
00747     {
00748       if (reg.ACCEL_FCHOICE != 1) retval |= ICM_20948_Stat_Err;
00749     }
00750     else
00751     {
00752       if (reg.ACCEL_FCHOICE != 0) retval |= ICM_20948_Stat_Err;
00753     }
00754   }
00755   if (sensors & ICM_20948_Internal_Gyr)
00756   {
00757     ICM_20948_GYRO_CONFIG_1_t reg;
00758     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00759     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00760     if (enable)
00761     {
00762       reg.GYRO_FCHOICE = 1;
00763     }
00764     else
00765     {
00766       reg.GYRO_FCHOICE = 0;
00767     }
00768     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00769     // Check the data was written correctly
00770     retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
00771     if (enable)
00772     {
00773       if (reg.GYRO_FCHOICE != 1) retval |= ICM_20948_Stat_Err;
00774     }
00775     else
00776     {
00777       if (reg.GYRO_FCHOICE != 0) retval |= ICM_20948_Stat_Err;
00778     }
00779   }
00780   return retval;
00781 }
00782 
00783 ICM_20948_Status_e ICM_20948_set_sample_rate(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt)
00784 {
00785   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00786 
00787   if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
00788   {
00789     return ICM_20948_Stat_SensorNotSupported;
00790   }
00791 
00792   if (sensors & ICM_20948_Internal_Acc)
00793   {
00794     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00795     uint8_t div1 = (smplrt.a << 8);
00796     uint8_t div2 = (smplrt.a & 0xFF);
00797     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1);
00798     retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1);
00799   }
00800   if (sensors & ICM_20948_Internal_Gyr)
00801   {
00802     retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
00803     uint8_t div = (smplrt.g);
00804     retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1);
00805   }
00806   return retval;
00807 }
00808 
00809 // Interface Things
00810 ICM_20948_Status_e ICM_20948_i2c_master_passthrough(ICM_20948_Device_t *pdev, bool passthrough)
00811 {
00812   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00813 
00814   ICM_20948_INT_PIN_CFG_t reg;
00815   retval = ICM_20948_set_bank(pdev, 0);
00816   if (retval != ICM_20948_Stat_Ok)
00817   {
00818     return retval;
00819   }
00820   retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
00821   if (retval != ICM_20948_Stat_Ok)
00822   {
00823     return retval;
00824   }
00825   reg.BYPASS_EN = passthrough;
00826   retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
00827   if (retval != ICM_20948_Stat_Ok)
00828   {
00829     return retval;
00830   }
00831 
00832   return retval;
00833 }
00834 
00835 ICM_20948_Status_e ICM_20948_i2c_master_enable(ICM_20948_Device_t *pdev, bool enable)
00836 {
00837   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00838 
00839   // Disable BYPASS_EN
00840   retval = ICM_20948_i2c_master_passthrough(pdev, false);
00841   if (retval != ICM_20948_Stat_Ok)
00842   {
00843     return retval;
00844   }
00845 
00846   ICM_20948_I2C_MST_CTRL_t ctrl;
00847   retval = ICM_20948_set_bank(pdev, 3);
00848   if (retval != ICM_20948_Stat_Ok)
00849   {
00850     return retval;
00851   }
00852   retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
00853   if (retval != ICM_20948_Stat_Ok)
00854   {
00855     return retval;
00856   }
00857   ctrl.I2C_MST_CLK = 0x07; // corresponds to 345.6 kHz, good for up to 400 kHz
00858   ctrl.I2C_MST_P_NSR = 1;
00859   retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
00860   if (retval != ICM_20948_Stat_Ok)
00861   {
00862     return retval;
00863   }
00864 
00865   ICM_20948_USER_CTRL_t reg;
00866   retval = ICM_20948_set_bank(pdev, 0);
00867   if (retval != ICM_20948_Stat_Ok)
00868   {
00869     return retval;
00870   }
00871   retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
00872   if (retval != ICM_20948_Stat_Ok)
00873   {
00874     return retval;
00875   }
00876   if (enable)
00877   {
00878     reg.I2C_MST_EN = 1;
00879   }
00880   else
00881   {
00882     reg.I2C_MST_EN = 0;
00883   }
00884   retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
00885   if (retval != ICM_20948_Stat_Ok)
00886   {
00887     return retval;
00888   }
00889 
00890   return retval;
00891 }
00892 
00893 ICM_20948_Status_e ICM_20948_i2c_master_reset(ICM_20948_Device_t *pdev)
00894 {
00895   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00896 
00897   ICM_20948_USER_CTRL_t ctrl;
00898   retval = ICM_20948_set_bank(pdev, 0);
00899   if (retval != ICM_20948_Stat_Ok)
00900   {
00901     return retval;
00902   }
00903 
00904   retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
00905   if (retval != ICM_20948_Stat_Ok)
00906   {
00907     return retval;
00908   }
00909 
00910   ctrl.I2C_MST_RST = 1; //Reset!
00911 
00912   retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
00913   if (retval != ICM_20948_Stat_Ok)
00914   {
00915     return retval;
00916   }
00917   return retval;
00918 }
00919 
00920 ICM_20948_Status_e ICM_20948_i2c_controller_configure_peripheral(ICM_20948_Device_t *pdev, uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut)
00921 {
00922   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
00923 
00924   uint8_t periph_addr_reg;
00925   uint8_t periph_reg_reg;
00926   uint8_t periph_ctrl_reg;
00927   uint8_t periph_do_reg;
00928 
00929   switch (peripheral)
00930   {
00931   case 0:
00932     periph_addr_reg = AGB3_REG_I2C_PERIPH0_ADDR;
00933     periph_reg_reg = AGB3_REG_I2C_PERIPH0_REG;
00934     periph_ctrl_reg = AGB3_REG_I2C_PERIPH0_CTRL;
00935     periph_do_reg = AGB3_REG_I2C_PERIPH0_DO;
00936     break;
00937   case 1:
00938     periph_addr_reg = AGB3_REG_I2C_PERIPH1_ADDR;
00939     periph_reg_reg = AGB3_REG_I2C_PERIPH1_REG;
00940     periph_ctrl_reg = AGB3_REG_I2C_PERIPH1_CTRL;
00941     periph_do_reg = AGB3_REG_I2C_PERIPH1_DO;
00942     break;
00943   case 2:
00944     periph_addr_reg = AGB3_REG_I2C_PERIPH2_ADDR;
00945     periph_reg_reg = AGB3_REG_I2C_PERIPH2_REG;
00946     periph_ctrl_reg = AGB3_REG_I2C_PERIPH2_CTRL;
00947     periph_do_reg = AGB3_REG_I2C_PERIPH2_DO;
00948     break;
00949   case 3:
00950     periph_addr_reg = AGB3_REG_I2C_PERIPH3_ADDR;
00951     periph_reg_reg = AGB3_REG_I2C_PERIPH3_REG;
00952     periph_ctrl_reg = AGB3_REG_I2C_PERIPH3_CTRL;
00953     periph_do_reg = AGB3_REG_I2C_PERIPH3_DO;
00954     break;
00955   default:
00956     return ICM_20948_Stat_ParamErr;
00957   }
00958 
00959   retval = ICM_20948_set_bank(pdev, 3);
00960   if (retval != ICM_20948_Stat_Ok)
00961   {
00962     return retval;
00963   }
00964 
00965   // Set the peripheral address and the Rw flag
00966   ICM_20948_I2C_PERIPHX_ADDR_t address;
00967   address.ID = addr;
00968   if (Rw)
00969   {
00970     address.RNW = 1;
00971   }
00972   else
00973   {
00974     address.RNW = 0; // Make sure bit is clear (just in case there is any garbage in that RAM location)
00975   }
00976   retval = ICM_20948_execute_w(pdev, periph_addr_reg, (uint8_t *)&address, sizeof(ICM_20948_I2C_PERIPHX_ADDR_t));
00977   if (retval != ICM_20948_Stat_Ok)
00978   {
00979     return retval;
00980   }
00981 
00982   // If we are setting up a write, configure the Data Out register too
00983   if (!Rw)
00984   {
00985     ICM_20948_I2C_PERIPHX_DO_t dataOutByte;
00986     dataOutByte.DO = dataOut;
00987     retval = ICM_20948_execute_w(pdev, periph_do_reg, (uint8_t *)&dataOutByte, sizeof(ICM_20948_I2C_PERIPHX_DO_t));
00988     if (retval != ICM_20948_Stat_Ok)
00989     {
00990       return retval;
00991     }
00992   }
00993 
00994   // Set the peripheral sub-address (register address)
00995   ICM_20948_I2C_PERIPHX_REG_t subaddress;
00996   subaddress.REG = reg;
00997   retval = ICM_20948_execute_w(pdev, periph_reg_reg, (uint8_t *)&subaddress, sizeof(ICM_20948_I2C_PERIPHX_REG_t));
00998   if (retval != ICM_20948_Stat_Ok)
00999   {
01000     return retval;
01001   }
01002 
01003   // Set up the control info
01004   ICM_20948_I2C_PERIPHX_CTRL_t ctrl;
01005   ctrl.LENG = len;
01006   ctrl.EN = enable;
01007   ctrl.REG_DIS = data_only;
01008   ctrl.GRP = grp;
01009   ctrl.BYTE_SW = swap;
01010   retval = ICM_20948_execute_w(pdev, periph_ctrl_reg, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_PERIPHX_CTRL_t));
01011   if (retval != ICM_20948_Stat_Ok)
01012   {
01013     return retval;
01014   }
01015 
01016   return retval;
01017 }
01018 
01019 // Higher Level
01020 ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t *pdev, ICM_20948_AGMT_t *pagmt)
01021 {
01022   if (pagmt == NULL)
01023   {
01024     return ICM_20948_Stat_ParamErr;
01025   }
01026 
01027   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01028   const uint8_t numbytes = 14 + 9; //Read Accel, gyro, temp, and 9 bytes of mag
01029   uint8_t buff[numbytes];
01030 
01031   // Get readings
01032   retval |= ICM_20948_set_bank(pdev, 0);
01033   retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes);
01034 
01035   pagmt->acc.axes.x = ((buff[0] << 8) | (buff[1] & 0xFF));
01036   pagmt->acc.axes.y = ((buff[2] << 8) | (buff[3] & 0xFF));
01037   pagmt->acc.axes.z = ((buff[4] << 8) | (buff[5] & 0xFF));
01038 
01039   pagmt->gyr.axes.x = ((buff[6] << 8) | (buff[7] & 0xFF));
01040   pagmt->gyr.axes.y = ((buff[8] << 8) | (buff[9] & 0xFF));
01041   pagmt->gyr.axes.z = ((buff[10] << 8) | (buff[11] & 0xFF));
01042 
01043   pagmt->tmp.val = ((buff[12] << 8) | (buff[13] & 0xFF));
01044 
01045   pagmt->magStat1 = buff[14];
01046   pagmt->mag.axes.x = ((buff[16] << 8) | (buff[15] & 0xFF)); //Mag data is read little endian
01047   pagmt->mag.axes.y = ((buff[18] << 8) | (buff[17] & 0xFF));
01048   pagmt->mag.axes.z = ((buff[20] << 8) | (buff[19] & 0xFF));
01049   pagmt->magStat2 = buff[22];
01050 
01051   // Get settings to be able to compute scaled values
01052   retval |= ICM_20948_set_bank(pdev, 2);
01053   ICM_20948_ACCEL_CONFIG_t acfg;
01054   retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t *)&acfg, 1 * sizeof(acfg));
01055   pagmt->fss.a = acfg.ACCEL_FS_SEL; // Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range
01056                                     // Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be...
01057   retval |= ICM_20948_set_bank(pdev, 2);
01058   ICM_20948_GYRO_CONFIG_1_t gcfg1;
01059   retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&gcfg1, 1 * sizeof(gcfg1));
01060   pagmt->fss.g = gcfg1.GYRO_FS_SEL;
01061   ICM_20948_ACCEL_CONFIG_2_t acfg2;
01062   retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t *)&acfg2, 1 * sizeof(acfg2));
01063 
01064   return retval;
01065 }
01066 
01067 // FIFO
01068 
01069 ICM_20948_Status_e ICM_20948_enable_FIFO(ICM_20948_Device_t *pdev, bool enable)
01070 {
01071   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01072 
01073   ICM_20948_USER_CTRL_t ctrl;
01074   retval = ICM_20948_set_bank(pdev, 0);
01075   if (retval != ICM_20948_Stat_Ok)
01076   {
01077     return retval;
01078   }
01079 
01080   retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01081   if (retval != ICM_20948_Stat_Ok)
01082   {
01083     return retval;
01084   }
01085 
01086   if (enable)
01087     ctrl.FIFO_EN = 1;
01088   else
01089     ctrl.FIFO_EN = 0;
01090 
01091   retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01092   if (retval != ICM_20948_Stat_Ok)
01093   {
01094     return retval;
01095   }
01096   return retval;
01097 }
01098 
01099 ICM_20948_Status_e ICM_20948_reset_FIFO(ICM_20948_Device_t *pdev)
01100 {
01101   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01102 
01103   ICM_20948_FIFO_RST_t ctrl;
01104   retval = ICM_20948_set_bank(pdev, 0);
01105   if (retval != ICM_20948_Stat_Ok)
01106   {
01107     return retval;
01108   }
01109 
01110   retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
01111   if (retval != ICM_20948_Stat_Ok)
01112   {
01113     return retval;
01114   }
01115 
01116   ctrl.FIFO_RESET = 0x1F; // Datasheet says "FIFO_RESET[4:0]"
01117 
01118   retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
01119   if (retval != ICM_20948_Stat_Ok)
01120   {
01121     return retval;
01122   }
01123 
01124   //delay ???
01125 
01126   ctrl.FIFO_RESET = 0x1E; // The InvenSense Nucleo examples write 0x1F followed by 0x1E
01127 
01128   retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_RST, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_RST_t));
01129   if (retval != ICM_20948_Stat_Ok)
01130   {
01131     return retval;
01132   }
01133 
01134   return retval;
01135 }
01136 
01137 ICM_20948_Status_e ICM_20948_set_FIFO_mode(ICM_20948_Device_t *pdev, bool snapshot)
01138 {
01139   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01140 
01141   ICM_20948_FIFO_MODE_t ctrl;
01142   retval = ICM_20948_set_bank(pdev, 0);
01143   if (retval != ICM_20948_Stat_Ok)
01144   {
01145     return retval;
01146   }
01147 
01148   retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_MODE, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_MODE_t));
01149   if (retval != ICM_20948_Stat_Ok)
01150   {
01151     return retval;
01152   }
01153 
01154   if (snapshot)
01155     ctrl.FIFO_MODE = 0x1F; // Datasheet says "FIFO_MODE[4:0]"
01156   else
01157     ctrl.FIFO_MODE = 0;
01158 
01159   retval = ICM_20948_execute_w(pdev, AGB0_REG_FIFO_MODE, (uint8_t *)&ctrl, sizeof(ICM_20948_FIFO_MODE_t));
01160   if (retval != ICM_20948_Stat_Ok)
01161   {
01162     return retval;
01163   }
01164   return retval;
01165 }
01166 
01167 ICM_20948_Status_e ICM_20948_get_FIFO_count(ICM_20948_Device_t *pdev, uint16_t *count)
01168 {
01169   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01170 
01171   ICM_20948_FIFO_COUNTH_t ctrlh;
01172   ICM_20948_FIFO_COUNTL_t ctrll;
01173   retval = ICM_20948_set_bank(pdev, 0);
01174   if (retval != ICM_20948_Stat_Ok)
01175   {
01176     return retval;
01177   }
01178 
01179   retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_COUNT_H, (uint8_t *)&ctrlh, sizeof(ICM_20948_FIFO_COUNTH_t));
01180   if (retval != ICM_20948_Stat_Ok)
01181   {
01182     return retval;
01183   }
01184 
01185   ctrlh.FIFO_COUNTH &= 0x1F; // Datasheet says "FIFO_CNT[12:8]"
01186 
01187   retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_COUNT_L, (uint8_t *)&ctrll, sizeof(ICM_20948_FIFO_COUNTL_t));
01188   if (retval != ICM_20948_Stat_Ok)
01189   {
01190     return retval;
01191   }
01192 
01193   *count = (((uint16_t)ctrlh.FIFO_COUNTH) << 8) | (uint16_t)ctrll.FIFO_COUNTL;
01194 
01195   return retval;
01196 }
01197 
01198 ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t *pdev, uint8_t *data, uint8_t len)
01199 {
01200   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01201 
01202   retval = ICM_20948_set_bank(pdev, 0);
01203   if (retval != ICM_20948_Stat_Ok)
01204   {
01205     return retval;
01206   }
01207 
01208   retval = ICM_20948_execute_r(pdev, AGB0_REG_FIFO_R_W, data, len);
01209   if (retval != ICM_20948_Stat_Ok)
01210   {
01211     return retval;
01212   }
01213 
01214   return retval;
01215 }
01216 
01217 // DMP
01218 
01219 ICM_20948_Status_e ICM_20948_enable_DMP(ICM_20948_Device_t *pdev, bool enable)
01220 {
01221   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01222 
01223   ICM_20948_USER_CTRL_t ctrl;
01224   retval = ICM_20948_set_bank(pdev, 0);
01225   if (retval != ICM_20948_Stat_Ok)
01226   {
01227     return retval;
01228   }
01229 
01230   retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01231   if (retval != ICM_20948_Stat_Ok)
01232   {
01233     return retval;
01234   }
01235 
01236   if (enable)
01237     ctrl.DMP_EN = 1;
01238   else
01239     ctrl.DMP_EN = 0;
01240 
01241   retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01242   if (retval != ICM_20948_Stat_Ok)
01243   {
01244     return retval;
01245   }
01246   return retval;
01247 }
01248 
01249 ICM_20948_Status_e ICM_20948_reset_DMP(ICM_20948_Device_t *pdev)
01250 {
01251   ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
01252 
01253   ICM_20948_USER_CTRL_t ctrl;
01254   retval = ICM_20948_set_bank(pdev, 0);
01255   if (retval != ICM_20948_Stat_Ok)
01256   {
01257     return retval;
01258   }
01259 
01260   retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01261   if (retval != ICM_20948_Stat_Ok)
01262   {
01263     return retval;
01264   }
01265 
01266   ctrl.DMP_RST = 1;
01267 
01268   retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
01269   if (retval != ICM_20948_Stat_Ok)
01270   {
01271     return retval;
01272   }
01273   return retval;
01274 }
01275 
01276 ICM_20948_Status_e ICM_20948_firmware_load(ICM_20948_Device_t *pdev)
01277 {
01278 #if defined(ICM_20948_USE_DMP)
01279   return (inv_icm20948_firmware_load(pdev, dmp3_image, sizeof(dmp3_image), DMP_LOAD_START));
01280 #else
01281   return ICM_20948_Stat_DMPNotSupported;
01282 #endif
01283 }
01284 
01285 /** @brief Loads the DMP firmware from SRAM
01286 * @param[in] data  pointer where the image
01287 * @param[in] size  size if the image
01288 * @param[in] load_addr  address to loading the image
01289 * @return 0 in case of success, -1 for any error
01290 */
01291 ICM_20948_Status_e inv_icm20948_firmware_load(ICM_20948_Device_t *pdev, const unsigned char *data_start, unsigned short size_start, unsigned short load_addr)
01292 {
01293   int write_size;
01294   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01295   unsigned short memaddr;
01296   const unsigned char *data;
01297   unsigned short size;
01298   unsigned char data_cmp[INV_MAX_SERIAL_READ];
01299   int flag = 0;
01300 
01301   if (pdev->_dmp_firmware_available == false)
01302     return ICM_20948_Stat_DMPNotSupported;
01303 
01304   if (pdev->_firmware_loaded)
01305     return ICM_20948_Stat_Ok; // Bail with no error if firmware is already loaded
01306 
01307   result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
01308   if (result != ICM_20948_Stat_Ok)
01309   {
01310     return result;
01311   }
01312 
01313   result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
01314   if (result != ICM_20948_Stat_Ok)
01315   {
01316     return result;
01317   }
01318 
01319   // Write DMP memory
01320 
01321   data = data_start;
01322   size = size_start;
01323   memaddr = load_addr;
01324   #ifdef ICM_20948_USE_PROGMEM_FOR_DMP
01325   unsigned char data_not_pg[INV_MAX_SERIAL_READ]; // Suggested by @HyperKokichi in Issue #63
01326   #endif
01327   while (size > 0)
01328   {
01329     //write_size = min(size, INV_MAX_SERIAL_WRITE); // Write in chunks of INV_MAX_SERIAL_WRITE
01330     if (size <= INV_MAX_SERIAL_WRITE) // Write in chunks of INV_MAX_SERIAL_WRITE
01331       write_size = size;
01332     else
01333       write_size = INV_MAX_SERIAL_WRITE;
01334     if ((memaddr & 0xff) + write_size > 0x100)
01335     {
01336       // Moved across a bank
01337       write_size = (memaddr & 0xff) + write_size - 0x100;
01338     }
01339 #ifdef ICM_20948_USE_PROGMEM_FOR_DMP
01340     memcpy_P(data_not_pg, data, write_size);  // Suggested by @HyperKokichi in Issue #63
01341     result = inv_icm20948_write_mems(pdev, memaddr, write_size, (unsigned char *)data_not_pg);
01342 #else
01343     result = inv_icm20948_write_mems(pdev, memaddr, write_size, (unsigned char *)data);
01344 #endif
01345     if (result != ICM_20948_Stat_Ok)
01346       return result;
01347     data += write_size;
01348     size -= write_size;
01349     memaddr += write_size;
01350   }
01351 
01352   // Verify DMP memory
01353 
01354   data = data_start;
01355   size = size_start;
01356   memaddr = load_addr;
01357   while (size > 0)
01358   {
01359     //write_size = min(size, INV_MAX_SERIAL_READ); // Read in chunks of INV_MAX_SERIAL_READ
01360     if (size <= INV_MAX_SERIAL_READ) // Read in chunks of INV_MAX_SERIAL_READ
01361       write_size = size;
01362     else
01363       write_size = INV_MAX_SERIAL_READ;
01364     if ((memaddr & 0xff) + write_size > 0x100)
01365     {
01366       // Moved across a bank
01367       write_size = (memaddr & 0xff) + write_size - 0x100;
01368     }
01369     result = inv_icm20948_read_mems(pdev, memaddr, write_size, data_cmp);
01370     if (result != ICM_20948_Stat_Ok)
01371       flag++;                               // Error, DMP not written correctly
01372 #ifdef ICM_20948_USE_PROGMEM_FOR_DMP
01373     memcpy_P(data_not_pg, data, write_size);  // Suggested by @HyperKokichi in Issue #63
01374     if (memcmp(data_cmp, data_not_pg, write_size))
01375 #else
01376     if (memcmp(data_cmp, data, write_size)) // Compare the data
01377 #endif
01378       return ICM_20948_Stat_DMPVerifyFail;
01379     data += write_size;
01380     size -= write_size;
01381     memaddr += write_size;
01382   }
01383 
01384   //Enable LP_EN since we disabled it at begining of this function.
01385   result = ICM_20948_low_power(pdev, true); // Put chip into low power state
01386   if (result != ICM_20948_Stat_Ok)
01387     return result;
01388 
01389   if (!flag)
01390   {
01391     //Serial.println("DMP Firmware was updated successfully..");
01392     pdev->_firmware_loaded = true;
01393   }
01394 
01395   return result;
01396 }
01397 
01398 ICM_20948_Status_e ICM_20948_set_dmp_start_address(ICM_20948_Device_t *pdev, unsigned short address)
01399 {
01400   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01401 
01402   if (pdev->_dmp_firmware_available == false)
01403     return ICM_20948_Stat_DMPNotSupported;
01404 
01405   unsigned char start_address[2];
01406 
01407   start_address[0] = (unsigned char)(address >> 8);
01408   start_address[1] = (unsigned char)(address & 0xff);
01409 
01410   // result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
01411   // if (result != ICM_20948_Stat_Ok)
01412   // {
01413   //        return result;
01414   // }
01415   //
01416   // result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
01417   // if (result != ICM_20948_Stat_Ok)
01418   // {
01419   //        return result;
01420   // }
01421 
01422   result = ICM_20948_set_bank(pdev, 2); // Set bank 2
01423   if (result != ICM_20948_Stat_Ok)
01424   {
01425     return result;
01426   }
01427 
01428   // Write the sensor control bits into memory address AGB2_REG_PRGM_START_ADDRH
01429   result = ICM_20948_execute_w(pdev, AGB2_REG_PRGM_START_ADDRH, (uint8_t *)start_address, 2);
01430 
01431   return result;
01432 }
01433 
01434 /**
01435 *  @brief       Write data to a register in DMP memory
01436 *  @param[in]   DMP memory address
01437 *  @param[in]   number of byte to be written
01438 *  @param[out]  output data from the register
01439 *  @return     0 if successful.
01440 */
01441 ICM_20948_Status_e inv_icm20948_write_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, const unsigned char *data)
01442 {
01443   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01444   unsigned int bytesWritten = 0;
01445   unsigned int thisLen;
01446   unsigned char lBankSelected;
01447   unsigned char lStartAddrSelected;
01448 
01449   if (!data)
01450   {
01451     return ICM_20948_Stat_NoData;
01452   }
01453 
01454   result = ICM_20948_set_bank(pdev, 0); // Set bank 0
01455   if (result != ICM_20948_Stat_Ok)
01456   {
01457     return result;
01458   }
01459 
01460   lBankSelected = (reg >> 8);
01461 
01462   if (lBankSelected != pdev->_last_mems_bank)
01463   {
01464     pdev->_last_mems_bank = lBankSelected;
01465     result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
01466     if (result != ICM_20948_Stat_Ok)
01467     {
01468       return result;
01469     }
01470   }
01471 
01472   while (bytesWritten < length)
01473   {
01474     lStartAddrSelected = (reg & 0xff);
01475 
01476     /* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register).
01477            Contents are changed after read or write of the selected memory.
01478            This register must be written prior to each access to initialize the register to the proper starting address.
01479            The address will auto increment during burst transactions.  Two consecutive bursts without re-initializing the start address would skip one address. */
01480 
01481     result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_START_ADDR, &lStartAddrSelected, 1);
01482     if (result != ICM_20948_Stat_Ok)
01483     {
01484       return result;
01485     }
01486 
01487     if (length - bytesWritten <= INV_MAX_SERIAL_WRITE)
01488       thisLen = length - bytesWritten;
01489     else
01490       thisLen = INV_MAX_SERIAL_WRITE;
01491 
01492     /* Write data */
01493 
01494     result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_R_W, (uint8_t *)&data[bytesWritten], thisLen);
01495     if (result != ICM_20948_Stat_Ok)
01496     {
01497       return result;
01498     }
01499 
01500     bytesWritten += thisLen;
01501     reg += thisLen;
01502   }
01503 
01504   return result;
01505 }
01506 
01507 /**
01508 *  @brief      Read data from a register in DMP memory
01509 *  @param[in]  DMP memory address
01510 *  @param[in]  number of byte to be read
01511 *  @param[in]  input data from the register
01512 *  @return     0 if successful.
01513 */
01514 ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, unsigned char *data)
01515 {
01516   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01517   unsigned int bytesRead = 0;
01518   unsigned int thisLen;
01519   unsigned char lBankSelected;
01520   unsigned char lStartAddrSelected;
01521 
01522   if (!data)
01523   {
01524     return ICM_20948_Stat_NoData;
01525   }
01526 
01527   result = ICM_20948_set_bank(pdev, 0); // Set bank 0
01528   if (result != ICM_20948_Stat_Ok)
01529   {
01530     return result;
01531   }
01532 
01533   lBankSelected = (reg >> 8);
01534 
01535   if (lBankSelected != pdev->_last_mems_bank)
01536   {
01537     pdev->_last_mems_bank = lBankSelected;
01538     result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
01539     if (result != ICM_20948_Stat_Ok)
01540     {
01541       return result;
01542     }
01543   }
01544 
01545   while (bytesRead < length)
01546   {
01547     lStartAddrSelected = (reg & 0xff);
01548 
01549     /* Sets the starting read or write address for the selected memory, inside of the selected page (see MEM_SEL Register).
01550            Contents are changed after read or write of the selected memory.
01551            This register must be written prior to each access to initialize the register to the proper starting address.
01552            The address will auto increment during burst transactions.  Two consecutive bursts without re-initializing the start address would skip one address. */
01553 
01554     result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_START_ADDR, &lStartAddrSelected, 1);
01555     if (result != ICM_20948_Stat_Ok)
01556     {
01557       return result;
01558     }
01559 
01560     if (length - bytesRead <= INV_MAX_SERIAL_READ)
01561       thisLen = length - bytesRead;
01562     else
01563       thisLen = INV_MAX_SERIAL_READ;
01564 
01565     /* Read data */
01566 
01567     result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesRead], thisLen);
01568     if (result != ICM_20948_Stat_Ok)
01569     {
01570       return result;
01571     }
01572 
01573     bytesRead += thisLen;
01574     reg += thisLen;
01575   }
01576 
01577   return result;
01578 }
01579 
01580 ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum DMP_ODR_Registers odr_reg, uint16_t interval)
01581 {
01582   // Set the ODR registers and clear the ODR counter
01583 
01584   // In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor.
01585   // Setting value can be calculated as follows:
01586   // Value = (DMP running rate (225Hz) / ODR ) - 1
01587   // E.g. For a 25Hz ODR rate, value= (225/25) -1 = 8.
01588 
01589   // During run-time, if an ODR is changed, the corresponding rate counter must be reset.
01590   // To reset, write 2-byte {0,0} to DMP using keys below for a particular sensor:
01591 
01592   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01593   ICM_20948_Status_e result2 = ICM_20948_Stat_Ok;
01594 
01595   if (pdev->_dmp_firmware_available == false)
01596     return ICM_20948_Stat_DMPNotSupported;
01597 
01598   unsigned char odr_reg_val[2];
01599   odr_reg_val[0] = (unsigned char)(interval >> 8);
01600   odr_reg_val[1] = (unsigned char)(interval & 0xff);
01601 
01602   unsigned char odr_count_zero[2] = {0x00, 0x00};
01603 
01604   result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
01605   if (result != ICM_20948_Stat_Ok)
01606   {
01607     return result;
01608   }
01609 
01610   result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
01611   if (result != ICM_20948_Stat_Ok)
01612   {
01613     return result;
01614   }
01615 
01616   switch (odr_reg)
01617   {
01618   case DMP_ODR_Reg_Cpass_Calibr:
01619   {
01620     result = inv_icm20948_write_mems(pdev, ODR_CPASS_CALIBR, 2, (const unsigned char *)&odr_reg_val);
01621     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS_CALIBR, 2, (const unsigned char *)&odr_count_zero);
01622   }
01623   break;
01624   case DMP_ODR_Reg_Gyro_Calibr:
01625   {
01626     result = inv_icm20948_write_mems(pdev, ODR_GYRO_CALIBR, 2, (const unsigned char *)&odr_reg_val);
01627     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO_CALIBR, 2, (const unsigned char *)&odr_count_zero);
01628   }
01629   break;
01630   case DMP_ODR_Reg_Pressure:
01631   {
01632     result = inv_icm20948_write_mems(pdev, ODR_PRESSURE, 2, (const unsigned char *)&odr_reg_val);
01633     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_PRESSURE, 2, (const unsigned char *)&odr_count_zero);
01634   }
01635   break;
01636   case DMP_ODR_Reg_Geomag:
01637   {
01638     result = inv_icm20948_write_mems(pdev, ODR_GEOMAG, 2, (const unsigned char *)&odr_reg_val);
01639     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GEOMAG, 2, (const unsigned char *)&odr_count_zero);
01640   }
01641   break;
01642   case DMP_ODR_Reg_PQuat6:
01643   {
01644     result = inv_icm20948_write_mems(pdev, ODR_PQUAT6, 2, (const unsigned char *)&odr_reg_val);
01645     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_PQUAT6, 2, (const unsigned char *)&odr_count_zero);
01646   }
01647   break;
01648   case DMP_ODR_Reg_Quat9:
01649   {
01650     result = inv_icm20948_write_mems(pdev, ODR_QUAT9, 2, (const unsigned char *)&odr_reg_val);
01651     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT9, 2, (const unsigned char *)&odr_count_zero);
01652   }
01653   break;
01654   case DMP_ODR_Reg_Quat6:
01655   {
01656     result = inv_icm20948_write_mems(pdev, ODR_QUAT6, 2, (const unsigned char *)&odr_reg_val);
01657     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT6, 2, (const unsigned char *)&odr_count_zero);
01658   }
01659   break;
01660   case DMP_ODR_Reg_ALS:
01661   {
01662     result = inv_icm20948_write_mems(pdev, ODR_ALS, 2, (const unsigned char *)&odr_reg_val);
01663     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_ALS, 2, (const unsigned char *)&odr_count_zero);
01664   }
01665   break;
01666   case DMP_ODR_Reg_Cpass:
01667   {
01668     result = inv_icm20948_write_mems(pdev, ODR_CPASS, 2, (const unsigned char *)&odr_reg_val);
01669     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS, 2, (const unsigned char *)&odr_count_zero);
01670   }
01671   break;
01672   case DMP_ODR_Reg_Gyro:
01673   {
01674     result = inv_icm20948_write_mems(pdev, ODR_GYRO, 2, (const unsigned char *)&odr_reg_val);
01675     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO, 2, (const unsigned char *)&odr_count_zero);
01676   }
01677   break;
01678   case DMP_ODR_Reg_Accel:
01679   {
01680     result = inv_icm20948_write_mems(pdev, ODR_ACCEL, 2, (const unsigned char *)&odr_reg_val);
01681     result2 = inv_icm20948_write_mems(pdev, ODR_CNTR_ACCEL, 2, (const unsigned char *)&odr_count_zero);
01682   }
01683   break;
01684   default:
01685     result = ICM_20948_Stat_InvalDMPRegister;
01686     break;
01687   }
01688 
01689   result = ICM_20948_low_power(pdev, true); // Put chip into low power state
01690   if (result != ICM_20948_Stat_Ok)
01691     return result;
01692 
01693   if (result2 > result)
01694     result = result2; // Return the highest error
01695 
01696   return result;
01697 }
01698 
01699 ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, int state)
01700 {
01701   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01702 
01703   uint16_t inv_event_control = 0; // Use this to store the value for MOTION_EVENT_CTL
01704   uint16_t data_rdy_status = 0;   // Use this to store the value for DATA_RDY_STATUS
01705 
01706   if (pdev->_dmp_firmware_available == false)
01707     return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
01708 
01709   uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
01710 
01711   if (androidSensor >= ANDROID_SENSOR_NUM_MAX)
01712     return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported (TO DO: Support B2S etc)
01713 
01714   // Convert the Android sensor into a bit mask for DATA_OUT_CTL1
01715   uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
01716   if (delta == 0xFFFF)
01717     return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
01718 
01719   // Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_0 / _enabled_Android_1
01720   unsigned long androidSensorAsBitMask;
01721   if (androidSensor < 32) // Sensors 0-31
01722   {
01723     androidSensorAsBitMask = 1L << androidSensor;
01724     if (state == 0) // Should we disable the sensor?
01725     {
01726       pdev->_enabled_Android_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
01727     }
01728     else
01729     {
01730       pdev->_enabled_Android_0 |= androidSensorAsBitMask; // Set the bit to enable the sensor
01731     }
01732   }
01733   else // Sensors 32-
01734   {
01735     androidSensorAsBitMask = 1L << (androidSensor - 32);
01736     if (state == 0) // Should we disable the sensor?
01737     {
01738       pdev->_enabled_Android_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor
01739     }
01740     else
01741     {
01742       pdev->_enabled_Android_1 |= androidSensorAsBitMask; // Set the bit to enable the sensor
01743     }
01744   }
01745 
01746   // Now we know androidSensor is valid, reconstruct the value for DATA_OUT_CTL1 from _enabled_Android_0 and _enabled_Android_0
01747   delta = 0; // Clear delta
01748   for (int i = 0; i < 32; i++)
01749   {
01750     androidSensorAsBitMask = 1L << i;
01751     if ((pdev->_enabled_Android_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) is enabled
01752     {
01753       delta |= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
01754     }
01755     if ((pdev->_enabled_Android_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) is enabled
01756     {
01757       delta |= inv_androidSensor_to_control_bits[i + 32]; // If it is, or the required bits into delta
01758     }
01759     // Also check which bits need to be set in the Data Ready Status and Motion Event Control registers
01760     // Compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
01761     if (((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0))
01762     {
01763       data_rdy_status |= DMP_Data_ready_Accel;
01764       inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
01765     }
01766     if (((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0))
01767     {
01768       data_rdy_status |= DMP_Data_ready_Gyro;
01769       inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
01770     }
01771     if (((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0) || ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0))
01772     {
01773       data_rdy_status |= DMP_Data_ready_Secondary_Compass;
01774       inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
01775     }
01776   }
01777 
01778   result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
01779   if (result != ICM_20948_Stat_Ok)
01780   {
01781     return result;
01782   }
01783 
01784   result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
01785   if (result != ICM_20948_Stat_Ok)
01786   {
01787     return result;
01788   }
01789 
01790   // Check if Accel, Gyro/Gyro_Calibr or Compass_Calibr/Quat9/GeoMag/Compass are to be enabled. If they are then we need to request the accuracy data via header2.
01791   uint16_t delta2 = 0;
01792   if ((delta & DMP_Data_Output_Control_1_Accel) > 0)
01793   {
01794     delta2 |= DMP_Data_Output_Control_2_Accel_Accuracy;
01795   }
01796   if (((delta & DMP_Data_Output_Control_1_Gyro_Calibr) > 0) || ((delta & DMP_Data_Output_Control_1_Gyro) > 0))
01797   {
01798     delta2 |= DMP_Data_Output_Control_2_Gyro_Accuracy;
01799   }
01800   if (((delta & DMP_Data_Output_Control_1_Compass_Calibr) > 0) || ((delta & DMP_Data_Output_Control_1_Compass) > 0) || ((delta & DMP_Data_Output_Control_1_Quat9) > 0) || ((delta & DMP_Data_Output_Control_1_Geomag) > 0))
01801   {
01802     delta2 |= DMP_Data_Output_Control_2_Compass_Accuracy;
01803   }
01804   // TO DO: Add DMP_Data_Output_Control_2_Pickup etc. if required
01805 
01806   // Write the sensor control bits into memory address DATA_OUT_CTL1
01807   unsigned char data_output_control_reg[2];
01808   data_output_control_reg[0] = (unsigned char)(delta >> 8);
01809   data_output_control_reg[1] = (unsigned char)(delta & 0xff);
01810   pdev->_dataOutCtl1 = delta; // Diagnostics
01811   result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL1, 2, (const unsigned char *)&data_output_control_reg);
01812   if (result != ICM_20948_Stat_Ok)
01813   {
01814     return result;
01815   }
01816 
01817   // Write the 'header2' sensor control bits into memory address DATA_OUT_CTL2
01818   data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
01819   data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
01820   pdev->_dataOutCtl2 = delta2; // Diagnostics
01821   result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL2, 2, (const unsigned char *)&data_output_control_reg);
01822   if (result != ICM_20948_Stat_Ok)
01823   {
01824     return result;
01825   }
01826 
01827   // Set the DATA_RDY_STATUS register
01828   data_output_control_reg[0] = (unsigned char)(data_rdy_status >> 8);
01829   data_output_control_reg[1] = (unsigned char)(data_rdy_status & 0xff);
01830   pdev->_dataRdyStatus = data_rdy_status; // Diagnostics
01831   result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
01832   if (result != ICM_20948_Stat_Ok)
01833   {
01834     return result;
01835   }
01836 
01837   // Check which extra bits need to be set in the Motion Event Control register
01838   if ((delta & DMP_Data_Output_Control_1_Quat9) > 0)
01839   {
01840     inv_event_control |= DMP_Motion_Event_Control_9axis;
01841   }
01842   if (((delta & DMP_Data_Output_Control_1_Step_Detector) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_0) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_1) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_2) > 0))
01843   {
01844     inv_event_control |= DMP_Motion_Event_Control_Pedometer_Interrupt;
01845   }
01846   if ((delta & DMP_Data_Output_Control_1_Geomag) > 0)
01847   {
01848     inv_event_control |= DMP_Motion_Event_Control_Geomag;
01849   }
01850 
01851   // Set the MOTION_EVENT_CTL register
01852   data_output_control_reg[0] = (unsigned char)(inv_event_control >> 8);
01853   data_output_control_reg[1] = (unsigned char)(inv_event_control & 0xff);
01854   pdev->_motionEventCtl = inv_event_control; // Diagnostics
01855   result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
01856   if (result != ICM_20948_Stat_Ok)
01857   {
01858     return result;
01859   }
01860 
01861   result = ICM_20948_low_power(pdev, true); // Put chip into low power state
01862   if (result != ICM_20948_Stat_Ok)
01863     return result;
01864 
01865   return result;
01866 }
01867 
01868 ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, int state)
01869 {
01870   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01871 
01872   if (pdev->_dmp_firmware_available == false)
01873     return ICM_20948_Stat_DMPNotSupported; // Bail if DMP is not supported
01874 
01875   uint8_t androidSensor = sensor_type_2_android_sensor(sensor); // Convert sensor from enum inv_icm20948_sensor to Android numbering
01876 
01877   if (androidSensor > ANDROID_SENSOR_NUM_MAX)
01878     return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
01879 
01880   // Convert the Android sensor into a bit mask for DATA_OUT_CTL1
01881   uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
01882   if (delta == 0xFFFF)
01883     return ICM_20948_Stat_SensorNotSupported; // Bail if the sensor is not supported
01884 
01885   // Convert the Android sensor number into a bitmask and set or clear that bit in _enabled_Android_intr_0 / _enabled_Android_intr_1
01886   unsigned long androidSensorAsBitMask;
01887   if (androidSensor < 32) // Sensors 0-31
01888   {
01889     androidSensorAsBitMask = 1L << androidSensor;
01890     if (state == 0) // Should we disable the sensor interrupt?
01891     {
01892       pdev->_enabled_Android_intr_0 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
01893     }
01894     else
01895     {
01896       pdev->_enabled_Android_intr_0 |= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
01897     }
01898   }
01899   else // Sensors 32-
01900   {
01901     androidSensorAsBitMask = 1L << (androidSensor - 32);
01902     if (state == 0) // Should we disable the sensor?
01903     {
01904       pdev->_enabled_Android_intr_1 &= ~androidSensorAsBitMask; // Clear the bit to disable the sensor interrupt
01905     }
01906     else
01907     {
01908       pdev->_enabled_Android_intr_1 |= androidSensorAsBitMask; // Set the bit to enable the sensor interrupt
01909     }
01910   }
01911 
01912   // Now we know androidSensor is valid, reconstruct the value for DATA_INTR_CTL from _enabled_Android_intr_0 and _enabled_Android_intr_0
01913   delta = 0; // Clear delta
01914   for (int i = 0; i < 32; i++)
01915   {
01916     androidSensorAsBitMask = 1L << i;
01917     if ((pdev->_enabled_Android_intr_0 & androidSensorAsBitMask) > 0) // Check if the Android sensor (0-31) interrupt is enabled
01918     {
01919       delta |= inv_androidSensor_to_control_bits[i]; // If it is, or the required bits into delta
01920     }
01921     if ((pdev->_enabled_Android_intr_1 & androidSensorAsBitMask) > 0) // Check if the Android sensor (32-) interrupt is enabled
01922     {
01923       delta |= inv_androidSensor_to_control_bits[i + 32]; // If it is, or the required bits into delta
01924     }
01925   }
01926 
01927   result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
01928   if (result != ICM_20948_Stat_Ok)
01929   {
01930     return result;
01931   }
01932 
01933   result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
01934   if (result != ICM_20948_Stat_Ok)
01935   {
01936     return result;
01937   }
01938 
01939   unsigned char data_intr_ctl[2];
01940 
01941   data_intr_ctl[0] = (unsigned char)(delta >> 8);
01942   data_intr_ctl[1] = (unsigned char)(delta & 0xff);
01943   pdev->_dataIntrCtl = delta; // Diagnostics
01944   
01945   // Write the interrupt control bits into memory address DATA_INTR_CTL
01946   result = inv_icm20948_write_mems(pdev, DATA_INTR_CTL, 2, (const unsigned char *)&data_intr_ctl);
01947 
01948   result = ICM_20948_low_power(pdev, true); // Put chip into low power state
01949   if (result != ICM_20948_Stat_Ok)
01950     return result;
01951 
01952   return result;
01953 }
01954 
01955 ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_20948_DMP_data_t *data)
01956 {
01957   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
01958   uint8_t fifoBytes[icm_20948_DMP_Maximum_Bytes]; // Interim storage for the FIFO data
01959 
01960   if (pdev->_dmp_firmware_available == false)
01961     return ICM_20948_Stat_DMPNotSupported;
01962 
01963   // Check how much data is in the FIFO
01964   uint16_t fifo_count;
01965   result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
01966   if (result != ICM_20948_Stat_Ok)
01967     return result;
01968 
01969   if (fifo_count < icm_20948_DMP_Header_Bytes) // Has a 2-byte header arrived?
01970     return ICM_20948_Stat_FIFONoDataAvail;     // Bail if no header is available
01971 
01972   // Read the header (2 bytes)
01973   data->header = 0; // Clear the existing header
01974   uint16_t aShort = 0;
01975   result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Header_Bytes);
01976   if (result != ICM_20948_Stat_Ok)
01977     return result;
01978   for (int i = 0; i < icm_20948_DMP_Header_Bytes; i++)
01979   {
01980     aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
01981   }
01982   data->header = aShort;                    // Store the header in data->header
01983   fifo_count -= icm_20948_DMP_Header_Bytes; // Decrement the count
01984 
01985   // If the header indicates a header2 is present then read that now
01986   data->header2 = 0;                                  // Clear the existing header2
01987   if ((data->header & DMP_header_bitmap_Header2) > 0) // If the header2 bit is set
01988   {
01989     if (fifo_count < icm_20948_DMP_Header2_Bytes) // Check if we need to read the FIFO count again
01990     {
01991       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
01992       if (result != ICM_20948_Stat_Ok)
01993         return result;
01994     }
01995     if (fifo_count < icm_20948_DMP_Header2_Bytes)
01996       return ICM_20948_Stat_FIFOIncompleteData; // Bail if no header2 is available
01997     // Read the header (2 bytes)
01998     aShort = 0;
01999     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Header2_Bytes);
02000     if (result != ICM_20948_Stat_Ok)
02001       return result;
02002     for (int i = 0; i < icm_20948_DMP_Header2_Bytes; i++)
02003     {
02004       aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02005     }
02006     data->header2 = aShort;                    // Store the header2 in data->header2
02007     fifo_count -= icm_20948_DMP_Header2_Bytes; // Decrement the count
02008   }
02009 
02010   if ((data->header & DMP_header_bitmap_Accel) > 0) // case DMP_header_bitmap_Accel:
02011   {
02012     if (fifo_count < icm_20948_DMP_Raw_Accel_Bytes) // Check if we need to read the FIFO count again
02013     {
02014       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02015       if (result != ICM_20948_Stat_Ok)
02016         return result;
02017     }
02018     if (fifo_count < icm_20948_DMP_Raw_Accel_Bytes)
02019       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02020     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Raw_Accel_Bytes);
02021     if (result != ICM_20948_Stat_Ok)
02022       return result;
02023     for (int i = 0; i < icm_20948_DMP_Raw_Accel_Bytes; i++)
02024     {
02025       data->Raw_Accel.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02026     }
02027     fifo_count -= icm_20948_DMP_Raw_Accel_Bytes; // Decrement the count
02028   }
02029 
02030   if ((data->header & DMP_header_bitmap_Gyro) > 0) // case DMP_header_bitmap_Gyro:
02031   {
02032     if (fifo_count < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes)) // Check if we need to read the FIFO count again
02033     {
02034       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02035       if (result != ICM_20948_Stat_Ok)
02036         return result;
02037     }
02038     if (fifo_count < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes))
02039       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02040     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes));
02041     if (result != ICM_20948_Stat_Ok)
02042       return result;
02043     for (int i = 0; i < (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes); i++)
02044     {
02045       data->Raw_Gyro.Bytes[DMP_Raw_Gyro_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02046     }
02047     fifo_count -= (icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes); // Decrement the count
02048   }
02049 
02050   if ((data->header & DMP_header_bitmap_Compass) > 0) // case DMP_header_bitmap_Compass:
02051   {
02052     if (fifo_count < icm_20948_DMP_Compass_Bytes) // Check if we need to read the FIFO count again
02053     {
02054       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02055       if (result != ICM_20948_Stat_Ok)
02056         return result;
02057     }
02058     if (fifo_count < icm_20948_DMP_Compass_Bytes)
02059       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02060     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Bytes);
02061     if (result != ICM_20948_Stat_Ok)
02062       return result;
02063     for (int i = 0; i < icm_20948_DMP_Compass_Bytes; i++)
02064     {
02065       data->Compass.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02066     }
02067     fifo_count -= icm_20948_DMP_Compass_Bytes; // Decrement the count
02068   }
02069 
02070   if ((data->header & DMP_header_bitmap_ALS) > 0) // case DMP_header_bitmap_ALS:
02071   {
02072     if (fifo_count < icm_20948_DMP_ALS_Bytes) // Check if we need to read the FIFO count again
02073     {
02074       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02075       if (result != ICM_20948_Stat_Ok)
02076         return result;
02077     }
02078     if (fifo_count < icm_20948_DMP_ALS_Bytes)
02079       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02080     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_ALS_Bytes);
02081     if (result != ICM_20948_Stat_Ok)
02082       return result;
02083     for (int i = 0; i < icm_20948_DMP_ALS_Bytes; i++)
02084     {
02085       data->ALS[i] = fifoBytes[i];
02086     }
02087     fifo_count -= icm_20948_DMP_ALS_Bytes; // Decrement the count
02088   }
02089 
02090   if ((data->header & DMP_header_bitmap_Quat6) > 0) // case DMP_header_bitmap_Quat6:
02091   {
02092     if (fifo_count < icm_20948_DMP_Quat6_Bytes) // Check if we need to read the FIFO count again
02093     {
02094       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02095       if (result != ICM_20948_Stat_Ok)
02096         return result;
02097     }
02098     if (fifo_count < icm_20948_DMP_Quat6_Bytes)
02099       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02100     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat6_Bytes);
02101     if (result != ICM_20948_Stat_Ok)
02102       return result;
02103     for (int i = 0; i < icm_20948_DMP_Quat6_Bytes; i++)
02104     {
02105       data->Quat6.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02106     }
02107     fifo_count -= icm_20948_DMP_Quat6_Bytes; // Decrement the count
02108   }
02109 
02110   if ((data->header & DMP_header_bitmap_Quat9) > 0) // case DMP_header_bitmap_Quat9:
02111   {
02112     if (fifo_count < icm_20948_DMP_Quat9_Bytes) // Check if we need to read the FIFO count again
02113     {
02114       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02115       if (result != ICM_20948_Stat_Ok)
02116         return result;
02117     }
02118     if (fifo_count < icm_20948_DMP_Quat9_Bytes)
02119       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02120     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Quat9_Bytes);
02121     if (result != ICM_20948_Stat_Ok)
02122       return result;
02123     for (int i = 0; i < icm_20948_DMP_Quat9_Bytes; i++)
02124     {
02125       data->Quat9.Bytes[DMP_Quat9_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02126     }
02127     fifo_count -= icm_20948_DMP_Quat9_Bytes; // Decrement the count
02128   }
02129 
02130   if ((data->header & DMP_header_bitmap_PQuat6) > 0) // case DMP_header_bitmap_PQuat6:
02131   {
02132     if (fifo_count < icm_20948_DMP_PQuat6_Bytes) // Check if we need to read the FIFO count again
02133     {
02134       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02135       if (result != ICM_20948_Stat_Ok)
02136         return result;
02137     }
02138     if (fifo_count < icm_20948_DMP_PQuat6_Bytes)
02139       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02140     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_PQuat6_Bytes);
02141     if (result != ICM_20948_Stat_Ok)
02142       return result;
02143     for (int i = 0; i < icm_20948_DMP_PQuat6_Bytes; i++)
02144     {
02145       data->PQuat6.Bytes[DMP_PQuat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02146     }
02147     fifo_count -= icm_20948_DMP_PQuat6_Bytes; // Decrement the count
02148   }
02149 
02150   if ((data->header & DMP_header_bitmap_Geomag) > 0) // case DMP_header_bitmap_Geomag:
02151   {
02152     if (fifo_count < icm_20948_DMP_Geomag_Bytes) // Check if we need to read the FIFO count again
02153     {
02154       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02155       if (result != ICM_20948_Stat_Ok)
02156         return result;
02157     }
02158     if (fifo_count < icm_20948_DMP_Geomag_Bytes)
02159       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02160     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Geomag_Bytes);
02161     if (result != ICM_20948_Stat_Ok)
02162       return result;
02163     for (int i = 0; i < icm_20948_DMP_Geomag_Bytes; i++)
02164     {
02165       data->Geomag.Bytes[DMP_Quat9_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02166     }
02167     fifo_count -= icm_20948_DMP_Geomag_Bytes; // Decrement the count
02168   }
02169 
02170   if ((data->header & DMP_header_bitmap_Pressure) > 0) // case DMP_header_bitmap_Pressure:
02171   {
02172     if (fifo_count < icm_20948_DMP_Pressure_Bytes) // Check if we need to read the FIFO count again
02173     {
02174       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02175       if (result != ICM_20948_Stat_Ok)
02176         return result;
02177     }
02178     if (fifo_count < icm_20948_DMP_Pressure_Bytes)
02179       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02180     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pressure_Bytes);
02181     if (result != ICM_20948_Stat_Ok)
02182       return result;
02183     for (int i = 0; i < icm_20948_DMP_Pressure_Bytes; i++)
02184     {
02185       data->Pressure[i] = fifoBytes[i];
02186     }
02187     fifo_count -= icm_20948_DMP_Pressure_Bytes; // Decrement the count
02188   }
02189 
02190   if ((data->header & DMP_header_bitmap_Gyro_Calibr) > 0) // case DMP_header_bitmap_Gyro_Calibr:
02191   {
02192     // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes is not supported
02193     // and looking at DMP frames which have the Gyro_Calibr bit set, that certainly seems to be true.
02194     // So, we'll skip this...:
02195     /*
02196             if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes) // Check if we need to read the FIFO count again
02197             {
02198                     result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02199                     if (result != ICM_20948_Stat_Ok)
02200                             return result;
02201             }
02202             if (fifo_count < icm_20948_DMP_Gyro_Calibr_Bytes)
02203                     return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02204             result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Calibr_Bytes);
02205             if (result != ICM_20948_Stat_Ok)
02206                     return result;
02207             for (int i = 0; i < icm_20948_DMP_Gyro_Calibr_Bytes; i++)
02208             {
02209                     data->Gyro_Calibr.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02210             }
02211             fifo_count -= icm_20948_DMP_Gyro_Calibr_Bytes; // Decrement the count
02212             */
02213   }
02214 
02215   if ((data->header & DMP_header_bitmap_Compass_Calibr) > 0) // case DMP_header_bitmap_Compass_Calibr:
02216   {
02217     if (fifo_count < icm_20948_DMP_Compass_Calibr_Bytes) // Check if we need to read the FIFO count again
02218     {
02219       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02220       if (result != ICM_20948_Stat_Ok)
02221         return result;
02222     }
02223     if (fifo_count < icm_20948_DMP_Compass_Calibr_Bytes)
02224       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02225     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Calibr_Bytes);
02226     if (result != ICM_20948_Stat_Ok)
02227       return result;
02228     for (int i = 0; i < icm_20948_DMP_Compass_Calibr_Bytes; i++)
02229     {
02230       data->Compass_Calibr.Bytes[DMP_Quat6_Byte_Ordering[i]] = fifoBytes[i]; // Correct the byte order (map big endian to little endian)
02231     }
02232     fifo_count -= icm_20948_DMP_Compass_Calibr_Bytes; // Decrement the count
02233   }
02234 
02235   if ((data->header & DMP_header_bitmap_Step_Detector) > 0) // case DMP_header_bitmap_Step_Detector:
02236   {
02237     if (fifo_count < icm_20948_DMP_Step_Detector_Bytes) // Check if we need to read the FIFO count again
02238     {
02239       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02240       if (result != ICM_20948_Stat_Ok)
02241         return result;
02242     }
02243     if (fifo_count < icm_20948_DMP_Step_Detector_Bytes)
02244       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02245     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Step_Detector_Bytes);
02246     if (result != ICM_20948_Stat_Ok)
02247       return result;
02248     uint32_t aWord = 0;
02249     for (int i = 0; i < icm_20948_DMP_Step_Detector_Bytes; i++)
02250     {
02251       aWord |= ((uint32_t)fifoBytes[i]) << (24 - (i * 8));
02252     }
02253     data->Pedometer_Timestamp = aWord;
02254     fifo_count -= icm_20948_DMP_Step_Detector_Bytes; // Decrement the count
02255   }
02256 
02257   // Now check for header2 features
02258 
02259   if ((data->header2 & DMP_header2_bitmap_Accel_Accuracy) > 0) // case DMP_header2_bitmap_Accel_Accuracy:
02260   {
02261     if (fifo_count < icm_20948_DMP_Accel_Accuracy_Bytes) // Check if we need to read the FIFO count again
02262     {
02263       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02264       if (result != ICM_20948_Stat_Ok)
02265         return result;
02266     }
02267     if (fifo_count < icm_20948_DMP_Accel_Accuracy_Bytes)
02268       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02269     aShort = 0;
02270     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Accel_Accuracy_Bytes);
02271     if (result != ICM_20948_Stat_Ok)
02272       return result;
02273     for (int i = 0; i < icm_20948_DMP_Accel_Accuracy_Bytes; i++)
02274     {
02275       aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02276     }
02277     data->Accel_Accuracy = aShort;
02278     fifo_count -= icm_20948_DMP_Accel_Accuracy_Bytes; // Decrement the count
02279   }
02280 
02281   if ((data->header2 & DMP_header2_bitmap_Gyro_Accuracy) > 0) // case DMP_header2_bitmap_Gyro_Accuracy:
02282   {
02283     if (fifo_count < icm_20948_DMP_Gyro_Accuracy_Bytes) // Check if we need to read the FIFO count again
02284     {
02285       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02286       if (result != ICM_20948_Stat_Ok)
02287         return result;
02288     }
02289     if (fifo_count < icm_20948_DMP_Gyro_Accuracy_Bytes)
02290       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02291     aShort = 0;
02292     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Gyro_Accuracy_Bytes);
02293     if (result != ICM_20948_Stat_Ok)
02294       return result;
02295     for (int i = 0; i < icm_20948_DMP_Gyro_Accuracy_Bytes; i++)
02296     {
02297       aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02298     }
02299     data->Gyro_Accuracy = aShort;
02300     fifo_count -= icm_20948_DMP_Gyro_Accuracy_Bytes; // Decrement the count
02301   }
02302 
02303   if ((data->header2 & DMP_header2_bitmap_Compass_Accuracy) > 0) // case DMP_header2_bitmap_Compass_Accuracy:
02304   {
02305     if (fifo_count < icm_20948_DMP_Compass_Accuracy_Bytes) // Check if we need to read the FIFO count again
02306     {
02307       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02308       if (result != ICM_20948_Stat_Ok)
02309         return result;
02310     }
02311     if (fifo_count < icm_20948_DMP_Compass_Accuracy_Bytes)
02312       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02313     aShort = 0;
02314     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Compass_Accuracy_Bytes);
02315     if (result != ICM_20948_Stat_Ok)
02316       return result;
02317     for (int i = 0; i < icm_20948_DMP_Compass_Accuracy_Bytes; i++)
02318     {
02319       aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02320     }
02321     data->Compass_Accuracy = aShort;
02322     fifo_count -= icm_20948_DMP_Compass_Accuracy_Bytes; // Decrement the count
02323   }
02324 
02325   if ((data->header2 & DMP_header2_bitmap_Fsync) > 0) // case DMP_header2_bitmap_Fsync:
02326   {
02327     // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes is not supported.
02328     // So, we'll skip this just in case...:
02329     /*
02330             if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes) // Check if we need to read the FIFO count again
02331             {
02332                     result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02333                     if (result != ICM_20948_Stat_Ok)
02334                             return result;
02335             }
02336             if (fifo_count < icm_20948_DMP_Fsync_Detection_Bytes)
02337                     return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02338             aShort = 0;
02339             result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Fsync_Detection_Bytes);
02340             if (result != ICM_20948_Stat_Ok)
02341                     return result;
02342             for (int i = 0; i < icm_20948_DMP_Fsync_Detection_Bytes; i++)
02343             {
02344                     aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02345             }
02346             data->Fsync_Delay_Time = aShort;
02347             fifo_count -= icm_20948_DMP_Fsync_Detection_Bytes; // Decrement the count
02348             */
02349   }
02350 
02351   if ((data->header2 & DMP_header2_bitmap_Pickup) > 0) // case DMP_header2_bitmap_Pickup:
02352   {
02353     if (fifo_count < icm_20948_DMP_Pickup_Bytes) // Check if we need to read the FIFO count again
02354     {
02355       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02356       if (result != ICM_20948_Stat_Ok)
02357         return result;
02358     }
02359     if (fifo_count < icm_20948_DMP_Pickup_Bytes)
02360       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02361     aShort = 0;
02362     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Pickup_Bytes);
02363     if (result != ICM_20948_Stat_Ok)
02364       return result;
02365     for (int i = 0; i < icm_20948_DMP_Pickup_Bytes; i++)
02366     {
02367       aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02368     }
02369     data->Pickup = aShort;
02370     fifo_count -= icm_20948_DMP_Pickup_Bytes; // Decrement the count
02371   }
02372 
02373   if ((data->header2 & DMP_header2_bitmap_Activity_Recog) > 0) // case DMP_header2_bitmap_Activity_Recog:
02374   {
02375     if (fifo_count < icm_20948_DMP_Activity_Recognition_Bytes) // Check if we need to read the FIFO count again
02376     {
02377       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02378       if (result != ICM_20948_Stat_Ok)
02379         return result;
02380     }
02381     if (fifo_count < icm_20948_DMP_Activity_Recognition_Bytes)
02382       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02383     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Activity_Recognition_Bytes);
02384     if (result != ICM_20948_Stat_Ok)
02385       return result;
02386     for (int i = 0; i < icm_20948_DMP_Activity_Recognition_Bytes; i++)
02387     {
02388       data->Activity_Recognition.Bytes[DMP_Activity_Recognition_Byte_Ordering[i]] = fifoBytes[i];
02389     }
02390     fifo_count -= icm_20948_DMP_Activity_Recognition_Bytes; // Decrement the count
02391   }
02392 
02393   if ((data->header2 & DMP_header2_bitmap_Secondary_On_Off) > 0) // case DMP_header2_bitmap_Secondary_On_Off:
02394   {
02395     if (fifo_count < icm_20948_DMP_Secondary_On_Off_Bytes) // Check if we need to read the FIFO count again
02396     {
02397       result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02398       if (result != ICM_20948_Stat_Ok)
02399         return result;
02400     }
02401     if (fifo_count < icm_20948_DMP_Secondary_On_Off_Bytes)
02402       return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02403     result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Secondary_On_Off_Bytes);
02404     if (result != ICM_20948_Stat_Ok)
02405       return result;
02406     for (int i = 0; i < icm_20948_DMP_Secondary_On_Off_Bytes; i++)
02407     {
02408       data->Secondary_On_Off.Bytes[DMP_Secondary_On_Off_Byte_Ordering[i]] = fifoBytes[i];
02409     }
02410     fifo_count -= icm_20948_DMP_Secondary_On_Off_Bytes; // Decrement the count
02411   }
02412 
02413   // Finally, extract the footer (gyro count)
02414   if (fifo_count < icm_20948_DMP_Footer_Bytes) // Check if we need to read the FIFO count again
02415   {
02416     result = ICM_20948_get_FIFO_count(pdev, &fifo_count);
02417     if (result != ICM_20948_Stat_Ok)
02418       return result;
02419   }
02420   if (fifo_count < icm_20948_DMP_Footer_Bytes)
02421     return ICM_20948_Stat_FIFOIncompleteData; // Bail if not enough data is available
02422   aShort = 0;
02423   result = ICM_20948_read_FIFO(pdev, &fifoBytes[0], icm_20948_DMP_Footer_Bytes);
02424   if (result != ICM_20948_Stat_Ok)
02425     return result;
02426   for (int i = 0; i < icm_20948_DMP_Footer_Bytes; i++)
02427   {
02428     aShort |= ((uint16_t)fifoBytes[i]) << (8 - (i * 8));
02429   }
02430   data->Footer = aShort;
02431   fifo_count -= icm_20948_DMP_Footer_Bytes; // Decrement the count
02432 
02433   if (fifo_count > 0) // Check if there is still data waiting to be read
02434     return ICM_20948_Stat_FIFOMoreDataAvail;
02435 
02436   return result;
02437 }
02438 
02439 uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor)
02440 {
02441   switch (sensor)
02442   {
02443   case INV_ICM20948_SENSOR_ACCELEROMETER:
02444     return ANDROID_SENSOR_ACCELEROMETER; // 1
02445   case INV_ICM20948_SENSOR_GYROSCOPE:
02446     return ANDROID_SENSOR_GYROSCOPE; // 4
02447   case INV_ICM20948_SENSOR_RAW_ACCELEROMETER:
02448     return ANDROID_SENSOR_RAW_ACCELEROMETER; // 42
02449   case INV_ICM20948_SENSOR_RAW_GYROSCOPE:
02450     return ANDROID_SENSOR_RAW_GYROSCOPE; // 43
02451   case INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
02452     return ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED; // 14
02453   case INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED:
02454     return ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED; // 16
02455   case INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON:
02456     return ANDROID_SENSOR_ACTIVITY_CLASSIFICATON; // 47
02457   case INV_ICM20948_SENSOR_STEP_DETECTOR:
02458     return ANDROID_SENSOR_STEP_DETECTOR; // 18
02459   case INV_ICM20948_SENSOR_STEP_COUNTER:
02460     return ANDROID_SENSOR_STEP_COUNTER; // 19
02461   case INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR:
02462     return ANDROID_SENSOR_GAME_ROTATION_VECTOR; // 15
02463   case INV_ICM20948_SENSOR_ROTATION_VECTOR:
02464     return ANDROID_SENSOR_ROTATION_VECTOR; // 11
02465   case INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
02466     return ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR; // 20
02467   case INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD:
02468     return ANDROID_SENSOR_GEOMAGNETIC_FIELD; // 2
02469   case INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION:
02470     return ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION; // 17
02471   case INV_ICM20948_SENSOR_FLIP_PICKUP:
02472     return ANDROID_SENSOR_FLIP_PICKUP; // 46
02473   case INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR:
02474     return ANDROID_SENSOR_WAKEUP_TILT_DETECTOR; // 41
02475   case INV_ICM20948_SENSOR_GRAVITY:
02476     return ANDROID_SENSOR_GRAVITY; // 9
02477   case INV_ICM20948_SENSOR_LINEAR_ACCELERATION:
02478     return ANDROID_SENSOR_LINEAR_ACCELERATION; // 10
02479   case INV_ICM20948_SENSOR_ORIENTATION:
02480     return ANDROID_SENSOR_ORIENTATION; // 3
02481   case INV_ICM20948_SENSOR_B2S:
02482     return ANDROID_SENSOR_B2S; // 45
02483   default:
02484     return ANDROID_SENSOR_NUM_MAX;
02485   }
02486 }
02487 
02488 enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor)
02489 {
02490   switch (sensor)
02491   {
02492   case ANDROID_SENSOR_ACCELEROMETER:
02493     return INV_ICM20948_SENSOR_ACCELEROMETER;
02494   case ANDROID_SENSOR_GYROSCOPE:
02495     return INV_ICM20948_SENSOR_GYROSCOPE;
02496   case ANDROID_SENSOR_RAW_ACCELEROMETER:
02497     return INV_ICM20948_SENSOR_RAW_ACCELEROMETER;
02498   case ANDROID_SENSOR_RAW_GYROSCOPE:
02499     return INV_ICM20948_SENSOR_RAW_GYROSCOPE;
02500   case ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
02501     return INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED;
02502   case ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED:
02503     return INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED;
02504   case ANDROID_SENSOR_ACTIVITY_CLASSIFICATON:
02505     return INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON;
02506   case ANDROID_SENSOR_STEP_DETECTOR:
02507     return INV_ICM20948_SENSOR_STEP_DETECTOR;
02508   case ANDROID_SENSOR_STEP_COUNTER:
02509     return INV_ICM20948_SENSOR_STEP_COUNTER;
02510   case ANDROID_SENSOR_GAME_ROTATION_VECTOR:
02511     return INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR;
02512   case ANDROID_SENSOR_ROTATION_VECTOR:
02513     return INV_ICM20948_SENSOR_ROTATION_VECTOR;
02514   case ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
02515     return INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR;
02516   case ANDROID_SENSOR_GEOMAGNETIC_FIELD:
02517     return INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD;
02518   case ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION:
02519     return INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION;
02520   case ANDROID_SENSOR_FLIP_PICKUP:
02521     return INV_ICM20948_SENSOR_FLIP_PICKUP;
02522   case ANDROID_SENSOR_WAKEUP_TILT_DETECTOR:
02523     return INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR;
02524   case ANDROID_SENSOR_GRAVITY:
02525     return INV_ICM20948_SENSOR_GRAVITY;
02526   case ANDROID_SENSOR_LINEAR_ACCELERATION:
02527     return INV_ICM20948_SENSOR_LINEAR_ACCELERATION;
02528   case ANDROID_SENSOR_ORIENTATION:
02529     return INV_ICM20948_SENSOR_ORIENTATION;
02530   case ANDROID_SENSOR_B2S:
02531     return INV_ICM20948_SENSOR_B2S;
02532   default:
02533     return INV_ICM20948_SENSOR_MAX;
02534   }
02535 }
02536 
02537 ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level)
02538 {
02539   ICM_20948_Status_e result = ICM_20948_Stat_Ok;
02540 
02541   if (pdev->_dmp_firmware_available == false)
02542     return ICM_20948_Stat_DMPNotSupported;
02543 
02544   // gyro_level should be set to 4 regardless of fullscale, due to the addition of API dmp_icm20648_set_gyro_fsr()
02545   gyro_level = 4;
02546 
02547   // First read the TIMEBASE_CORRECTION_PLL register from Bank 1
02548   int8_t pll; // Signed. Typical value is 0x18
02549   result = ICM_20948_set_bank(pdev, 1);
02550   result = ICM_20948_execute_r(pdev, AGB1_REG_TIMEBASE_CORRECTION_PLL, (uint8_t *)&pll, 1);
02551   if (result != ICM_20948_Stat_Ok)
02552   {
02553     return result;
02554   }
02555 
02556   pdev->_gyroSFpll = pll; // Record the PLL value so we can debug print it
02557 
02558   // Now calculate the Gyro SF using code taken from the InvenSense example (inv_icm20948_set_gyro_sf)
02559 
02560   long gyro_sf;
02561 
02562   unsigned long long const MagicConstant = 264446880937391LL;
02563   unsigned long long const MagicConstantScale = 100000LL;
02564   unsigned long long ResultLL;
02565 
02566   if (pll & 0x80)
02567   {
02568     ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 - (pll & 0x7F)) / MagicConstantScale);
02569   }
02570   else
02571   {
02572     ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 + pll) / MagicConstantScale);
02573   }
02574   /*
02575         In above deprecated FP version, worst case arguments can produce a result that overflows a signed long.
02576         Here, for such cases, we emulate the FP behavior of setting the result to the maximum positive value, as
02577         the compiler's conversion of a u64 to an s32 is simple truncation of the u64's high half, sadly....
02578     */
02579   if (ResultLL > 0x7FFFFFFF)
02580     gyro_sf = 0x7FFFFFFF;
02581   else
02582     gyro_sf = (long)ResultLL;
02583 
02584   pdev->_gyroSF = gyro_sf; // Record value so we can debug print it
02585 
02586   // Finally, write the value to the DMP GYRO_SF register
02587   unsigned char gyro_sf_reg[4];
02588   gyro_sf_reg[0] = (unsigned char)(gyro_sf >> 24);
02589   gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
02590   gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
02591   gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
02592   result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
02593 
02594   return result;
02595 }