From c8fa3c451d03d67ef98b8c78444cc058a49ce325 Mon Sep 17 00:00:00 2001
From: ThreadsOfFate <the__guy@hotmail.co.uk>
Date: Sat, 12 Nov 2022 22:40:27 +0000
Subject: [PATCH 1/9] Added Bias saving and loading.

---
 src/ICM_20948.cpp      | 192 +++++++++++++++++++++++++++++++++++++++++
 src/ICM_20948.h        |  22 +++++
 src/util/ICM_20948_C.c |   6 +-
 3 files changed, 218 insertions(+), 2 deletions(-)

diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 9f40d52..8363e71 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -239,6 +239,196 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
   }
 }
 
+//Gyro Bias
+ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char gyro_bias_reg[4];
+    gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
+    gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
+    gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
+    gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasGyroY( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char gyro_bias_reg[4];
+    gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
+    gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
+    gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
+    gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char gyro_bias_reg[4];
+    gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
+    gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
+    gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
+    gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+//Accel Bias
+ICM_20948_Status_e ICM_20948::SetBiasAccelX( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char accel_bias_reg[4];
+    accel_bias_reg[0] = (unsigned char)(newValue >> 24);
+    accel_bias_reg[1] = (unsigned char)(newValue >> 16);
+    accel_bias_reg[2] = (unsigned char)(newValue >> 8);
+    accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasAccelY( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char accel_bias_reg[4];
+    accel_bias_reg[0] = (unsigned char)(newValue >> 24);
+    accel_bias_reg[1] = (unsigned char)(newValue >> 16);
+    accel_bias_reg[2] = (unsigned char)(newValue >> 8);
+    accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char accel_bias_reg[4];
+    accel_bias_reg[0] = (unsigned char)(newValue >> 24);
+    accel_bias_reg[1] = (unsigned char)(newValue >> 16);
+    accel_bias_reg[2] = (unsigned char)(newValue >> 8);
+    accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+//CPass Bias
+ICM_20948_Status_e ICM_20948::SetBiasCPassX( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char cpass_bias_reg[4];
+    cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
+    cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
+    cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
+    cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasCPassY( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char cpass_bias_reg[4];
+    cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
+    cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
+    cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
+    cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int32_t newValue)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char cpass_bias_reg[4];
+    cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
+    cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
+    cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
+    cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
+    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
+ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* bias)
+{
+    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+    unsigned char bias_data[4] = { 0 };
+    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
+    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    return result;
+}
+
 float ICM_20948::temp(void)
 {
   return getTempC(agmt.tmp.val);
@@ -1796,3 +1986,5 @@ ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len,
 
   return ICM_20948_Stat_Ok;
 }
+
+
diff --git a/src/ICM_20948.h b/src/ICM_20948.h
index 4eb62e1..4b652d9 100644
--- a/src/ICM_20948.h
+++ b/src/ICM_20948.h
@@ -173,6 +173,28 @@ class ICM_20948
 
   //DMP
 
+  //Gyro Bias
+  ICM_20948_Status_e SetBiasGyroX( int32_t newValue);
+  ICM_20948_Status_e SetBiasGyroY( int32_t newValue);
+  ICM_20948_Status_e SetBiasGyroZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasGyroX( int32_t* bias);
+  ICM_20948_Status_e GetBiasGyroY( int32_t* bias);
+  ICM_20948_Status_e GetBiasGyroZ( int32_t* bias);
+  //Accel Bias
+  ICM_20948_Status_e SetBiasAccelX( int32_t newValue);
+  ICM_20948_Status_e SetBiasAccelY( int32_t newValue);
+  ICM_20948_Status_e SetBiasAccelZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasAccelX( int32_t* bias);
+  ICM_20948_Status_e GetBiasAccelY( int32_t* bias);
+  ICM_20948_Status_e GetBiasAccelZ( int32_t* bias);
+  //CPass Bias
+  ICM_20948_Status_e SetBiasCPassX( int32_t newValue);
+  ICM_20948_Status_e SetBiasCPassY( int32_t newValue);
+  ICM_20948_Status_e SetBiasCPassZ( int32_t newValue);
+  ICM_20948_Status_e GetBiasCPassX( int32_t* bias);
+  ICM_20948_Status_e GetBiasCPassY( int32_t* bias);
+  ICM_20948_Status_e GetBiasCPassZ( int32_t* bias);
+
   // Done:
   //  Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
   //  Load Firmware
diff --git a/src/util/ICM_20948_C.c b/src/util/ICM_20948_C.c
index 8cf2e5a..1e95c68 100644
--- a/src/util/ICM_20948_C.c
+++ b/src/util/ICM_20948_C.c
@@ -10,7 +10,7 @@
 
 #if defined(ARDUINO_ARCH_MBED) // ARDUINO_ARCH_MBED (APOLLO3 v2) does not support or require pgmspace.h / PROGMEM
 const uint8_t dmp3_image[] = {
-#elif (defined(__AVR__) || defined(__arm__) || defined(__ARDUINO_ARC__)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
+#elif (defined(__AVR__) || defined(__arm__) || defined(__ARDUINO_ARC__) || defined(ESP8266)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
 #define ICM_20948_USE_PROGMEM_FOR_DMP
 #include <avr/pgmspace.h>
 const uint8_t dmp3_image[] PROGMEM = {
@@ -2589,7 +2589,9 @@ ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned c
   gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
   gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
   gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
-  result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
+  result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char*)&gyro_sf_reg);
 
   return result;
 }
+
+

From d3578e206149b1d28ebc0ec1ef26b0e7b734492e Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 10:17:47 +0000
Subject: [PATCH 2/9] Avoid any possible cast ambiguity

Yes, I know, this probably doesn't change anything... And may make the code slightly slower... But, in my mind, this is the least ambiguous way to convert the big endian bytes to int32_t.
---
 src/ICM_20948.cpp | 63 ++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 54 insertions(+), 9 deletions(-)

diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 8363e71..0d1367f 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -281,7 +281,12 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -290,7 +295,12 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -299,7 +309,12 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 //Accel Bias
@@ -344,7 +359,12 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -353,7 +373,12 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -362,7 +387,12 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 //CPass Bias
@@ -407,7 +437,12 @@ ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -416,7 +451,12 @@ ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 
@@ -425,7 +465,12 @@ ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* bias)
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
     result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
-    bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
+    union {
+      int32_t signed32;
+      uint32_t unsigned32;
+    } signedUnsigned32;
+    signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
+    *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
     return result;
 }
 

From e4395370d37e246c02280361da17c1a4efba1c1d Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 11:19:38 +0000
Subject: [PATCH 3/9] Correct capitalization

---
 src/ICM_20948.cpp | 36 ++++++++++++++++++------------------
 src/ICM_20948.h   | 36 ++++++++++++++++++------------------
 2 files changed, 36 insertions(+), 36 deletions(-)

diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 0d1367f..4245c5e 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -240,7 +240,7 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
 }
 
 //Gyro Bias
-ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasGyroX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -252,7 +252,7 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasGyroY( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasGyroY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -264,7 +264,7 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroY( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasGyroZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char gyro_bias_reg[4];
@@ -276,7 +276,7 @@ ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasGyroX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -290,7 +290,7 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasGyroY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -304,7 +304,7 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasGyroZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -318,7 +318,7 @@ ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
     return result;
 }
 //Accel Bias
-ICM_20948_Status_e ICM_20948::SetBiasAccelX( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasAccelX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -330,7 +330,7 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelX( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasAccelY( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasAccelY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -342,7 +342,7 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelY( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasAccelZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char accel_bias_reg[4];
@@ -354,7 +354,7 @@ ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasAccelX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -368,7 +368,7 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasAccelY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -382,7 +382,7 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasAccelZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -396,7 +396,7 @@ ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
     return result;
 }
 //CPass Bias
-ICM_20948_Status_e ICM_20948::SetBiasCPassX( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasCPassX( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -408,7 +408,7 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassX( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasCPassY( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasCPassY( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -420,7 +420,7 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassY( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int32_t newValue)
+ICM_20948_Status_e ICM_20948::setBiasCPassZ( int32_t newValue)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char cpass_bias_reg[4];
@@ -432,7 +432,7 @@ ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int32_t newValue)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasCPassX( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -446,7 +446,7 @@ ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasCPassY( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
@@ -460,7 +460,7 @@ ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
     return result;
 }
 
-ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* bias)
+ICM_20948_Status_e ICM_20948::getBiasCPassZ( int32_t* bias)
 {
     ICM_20948_Status_e result = ICM_20948_Stat_Ok;
     unsigned char bias_data[4] = { 0 };
diff --git a/src/ICM_20948.h b/src/ICM_20948.h
index 4b652d9..b625db3 100644
--- a/src/ICM_20948.h
+++ b/src/ICM_20948.h
@@ -174,26 +174,26 @@ class ICM_20948
   //DMP
 
   //Gyro Bias
-  ICM_20948_Status_e SetBiasGyroX( int32_t newValue);
-  ICM_20948_Status_e SetBiasGyroY( int32_t newValue);
-  ICM_20948_Status_e SetBiasGyroZ( int32_t newValue);
-  ICM_20948_Status_e GetBiasGyroX( int32_t* bias);
-  ICM_20948_Status_e GetBiasGyroY( int32_t* bias);
-  ICM_20948_Status_e GetBiasGyroZ( int32_t* bias);
+  ICM_20948_Status_e setBiasGyroX(int32_t newValue);
+  ICM_20948_Status_e setBiasGyroY(int32_t newValue);
+  ICM_20948_Status_e setBiasGyroZ(int32_t newValue);
+  ICM_20948_Status_e getBiasGyroX(int32_t* bias);
+  ICM_20948_Status_e getBiasGyroY(int32_t* bias);
+  ICM_20948_Status_e getBiasGyroZ(int32_t* bias);
   //Accel Bias
-  ICM_20948_Status_e SetBiasAccelX( int32_t newValue);
-  ICM_20948_Status_e SetBiasAccelY( int32_t newValue);
-  ICM_20948_Status_e SetBiasAccelZ( int32_t newValue);
-  ICM_20948_Status_e GetBiasAccelX( int32_t* bias);
-  ICM_20948_Status_e GetBiasAccelY( int32_t* bias);
-  ICM_20948_Status_e GetBiasAccelZ( int32_t* bias);
+  ICM_20948_Status_e setBiasAccelX(int32_t newValue);
+  ICM_20948_Status_e setBiasAccelY(int32_t newValue);
+  ICM_20948_Status_e setBiasAccelZ(int32_t newValue);
+  ICM_20948_Status_e getBiasAccelX(int32_t* bias);
+  ICM_20948_Status_e getBiasAccelY(int32_t* bias);
+  ICM_20948_Status_e getBiasAccelZ(int32_t* bias);
   //CPass Bias
-  ICM_20948_Status_e SetBiasCPassX( int32_t newValue);
-  ICM_20948_Status_e SetBiasCPassY( int32_t newValue);
-  ICM_20948_Status_e SetBiasCPassZ( int32_t newValue);
-  ICM_20948_Status_e GetBiasCPassX( int32_t* bias);
-  ICM_20948_Status_e GetBiasCPassY( int32_t* bias);
-  ICM_20948_Status_e GetBiasCPassZ( int32_t* bias);
+  ICM_20948_Status_e setBiasCPassX(int32_t newValue);
+  ICM_20948_Status_e setBiasCPassY(int32_t newValue);
+  ICM_20948_Status_e setBiasCPassZ(int32_t newValue);
+  ICM_20948_Status_e getBiasCPassX(int32_t* bias);
+  ICM_20948_Status_e getBiasCPassY(int32_t* bias);
+  ICM_20948_Status_e getBiasCPassZ(int32_t* bias);
 
   // Done:
   //  Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL

From 32c78ee289b88811beaa63eae0748886db55b9ad Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 11:32:18 +0000
Subject: [PATCH 4/9] Update keywords.txt

---
 keywords.txt | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/keywords.txt b/keywords.txt
index 7877145..7015cbe 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -98,6 +98,15 @@ readDMPdataFromFIFO	KEYWORD2
 setGyroSF	KEYWORD2
 initializeDMP	KEYWORD2
 begin	KEYWORD2
+setBiasGyroX	KEYWORD2
+setBiasGyroY	KEYWORD2
+setBiasGyroZ	KEYWORD2
+setBiasAccelX	KEYWORD2
+setBiasAccelY	KEYWORD2
+setBiasAccelZ	KEYWORD2
+setBiasCPassX	KEYWORD2
+setBiasCPassY	KEYWORD2
+setBiasCPassZ	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)

From 74ec8f0a02c5be909bd54b7503439ba1fd204c5b Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 11:33:15 +0000
Subject: [PATCH 5/9] Check DMP is supported. Update status.

---
 src/ICM_20948.cpp | 162 ++++++++++++++++++++++++++++++----------------
 1 file changed, 108 insertions(+), 54 deletions(-)

diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 4245c5e..a73f23a 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -242,236 +242,290 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
 //Gyro Bias
 ICM_20948_Status_e ICM_20948::setBiasGyroX( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char gyro_bias_reg[4];
     gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
     gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
     gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
     gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasGyroY( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char gyro_bias_reg[4];
     gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
     gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
     gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
     gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasGyroZ( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char gyro_bias_reg[4];
     gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
     gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
     gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
     gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasGyroX( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasGyroY( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasGyroZ( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 //Accel Bias
 ICM_20948_Status_e ICM_20948::setBiasAccelX( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char accel_bias_reg[4];
     accel_bias_reg[0] = (unsigned char)(newValue >> 24);
     accel_bias_reg[1] = (unsigned char)(newValue >> 16);
     accel_bias_reg[2] = (unsigned char)(newValue >> 8);
     accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasAccelY( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char accel_bias_reg[4];
     accel_bias_reg[0] = (unsigned char)(newValue >> 24);
     accel_bias_reg[1] = (unsigned char)(newValue >> 16);
     accel_bias_reg[2] = (unsigned char)(newValue >> 8);
     accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasAccelZ( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char accel_bias_reg[4];
     accel_bias_reg[0] = (unsigned char)(newValue >> 24);
     accel_bias_reg[1] = (unsigned char)(newValue >> 16);
     accel_bias_reg[2] = (unsigned char)(newValue >> 8);
     accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasAccelX( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasAccelY( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasAccelZ( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 //CPass Bias
 ICM_20948_Status_e ICM_20948::setBiasCPassX( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char cpass_bias_reg[4];
     cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
     cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
     cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
     cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasCPassY( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char cpass_bias_reg[4];
     cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
     cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
     cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
     cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::setBiasCPassZ( int32_t newValue)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char cpass_bias_reg[4];
     cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
     cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
     cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
     cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
-    result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
-    return result;
+    status = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasCPassX( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasCPassY( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 ICM_20948_Status_e ICM_20948::getBiasCPassZ( int32_t* bias)
 {
-    ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+  if (_device._dmp_firmware_available == true) // Is DMP supported?
+  {
     unsigned char bias_data[4] = { 0 };
-    result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
+    status = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
     union {
       int32_t signed32;
       uint32_t unsigned32;
     } signedUnsigned32;
     signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
     *bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
-    return result;
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
 }
 
 float ICM_20948::temp(void)

From 64b9a97890f6e083a8938071c0444b529157d862 Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 12:51:10 +0000
Subject: [PATCH 6/9] Create Example11_DMP_Bias_Save_Restore_ESP32.ino

---
 .../Example11_DMP_Bias_Save_Restore_ESP32.ino | 402 ++++++++++++++++++
 1 file changed, 402 insertions(+)
 create mode 100644 examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino

diff --git a/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino
new file mode 100644
index 0000000..b229790
--- /dev/null
+++ b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino
@@ -0,0 +1,402 @@
+/****************************************************************
+ * Example11_DMP_Bias_Save_Restore_ESP32.ino
+ * ICM 20948 Arduino Library Demo
+ * Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
+ * Periodically save the biases in ESP32 EEPROM
+ * Restore the biases at startup - if valid
+ * Paul Clark, November 14th, 2022
+ * Based on original code by the SlimeVR-Tracker contributors:
+ * https://github.com/SlimeVR/SlimeVR-Tracker-ESP/blob/83550d21ef748678704b3a11c0f7afb6f44258e4/src/sensors/icm20948sensor.cpp#L30
+ * https://github.com/PaulZC/SlimeVR-Tracker-ESP/blob/83550d21ef748678704b3a11c0f7afb6f44258e4/src/sensors/icm20948sensor.cpp#L40-L41
+ * 
+ * ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
+ * ** We are grateful to InvenSense for sharing this with us.
+ * 
+ * ** Important note: by default the DMP functionality is disabled in the library
+ * ** as the DMP firmware takes up 14301 Bytes of program memory.
+ * ** To use the DMP, you will need to:
+ * ** Edit ICM_20948_C.h
+ * ** Uncomment line 29: #define ICM_20948_USE_DMP
+ * ** Save changes
+ * ** If you are using Windows, you can find ICM_20948_C.h in:
+ * ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
+ *
+ * Please see License.md for the license information.
+ *
+ * Distributed as-is; no warranty is given.
+ ***************************************************************/
+
+#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
+
+//#define USE_SPI       // Uncomment this to use SPI
+
+#define SERIAL_PORT Serial
+
+#define SPI_PORT SPI // Your desired SPI port.       Used only when "USE_SPI" is defined
+#define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
+
+#define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
+
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
+
+#ifdef USE_SPI
+ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
+#else
+ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
+#endif
+
+// -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
+
+#include <EEPROM.h>
+
+// Define a storage struct for the biases. Include a non-zero header and a simple checksum
+struct biasStore
+{
+  int32_t header = 0x42;
+  int32_t biasGyroX = 0;
+  int32_t biasGyroY = 0;
+  int32_t biasGyroZ = 0;
+  int32_t biasAccelX = 0;
+  int32_t biasAccelY = 0;
+  int32_t biasAccelZ = 0;
+  int32_t biasCPassX = 0;
+  int32_t biasCPassY = 0;
+  int32_t biasCPassZ = 0;
+  int32_t sum = 0;
+};
+
+// -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
+
+void updateBiasStoreSum(biasStore *store) // Update the bias store checksum
+{
+  int32_t sum = store->header;
+  sum += store->biasGyroX;
+  sum += store->biasGyroY;
+  sum += store->biasGyroZ;
+  sum += store->biasAccelX;
+  sum += store->biasAccelY;
+  sum += store->biasAccelZ;
+  sum += store->biasCPassX;
+  sum += store->biasCPassY;
+  sum += store->biasCPassZ;
+  store->sum = sum;
+}
+
+bool isBiasStoreValid(biasStore *store) // Returns true if the header and checksum are valid
+{
+  int32_t sum = store->header;
+
+  if (sum != 0x42)
+    return false;
+
+  sum += store->biasGyroX;
+  sum += store->biasGyroY;
+  sum += store->biasGyroZ;
+  sum += store->biasAccelX;
+  sum += store->biasAccelY;
+  sum += store->biasAccelZ;
+  sum += store->biasCPassX;
+  sum += store->biasCPassY;
+  sum += store->biasCPassZ;
+
+  return (store->sum == sum);
+}
+
+void printBiases(biasStore *store)
+{
+  SERIAL_PORT.print(F("Gyro X: "));
+  SERIAL_PORT.print(store->biasGyroX);
+  SERIAL_PORT.print(F(" Gyro Y: "));
+  SERIAL_PORT.print(store->biasGyroY);
+  SERIAL_PORT.print(F(" Gyro Z: "));
+  SERIAL_PORT.println(store->biasGyroZ);
+  SERIAL_PORT.print(F("Accel X: "));
+  SERIAL_PORT.print(store->biasAccelX);
+  SERIAL_PORT.print(F(" Accel Y: "));
+  SERIAL_PORT.print(store->biasAccelY);
+  SERIAL_PORT.print(F(" Accel Z: "));
+  SERIAL_PORT.println(store->biasAccelZ);
+  SERIAL_PORT.print(F("CPass X: "));
+  SERIAL_PORT.print(store->biasCPassX);
+  SERIAL_PORT.print(F(" CPass Y: "));
+  SERIAL_PORT.print(store->biasCPassY);
+  SERIAL_PORT.print(F(" CPass Z: "));
+  SERIAL_PORT.println(store->biasCPassZ);
+
+}
+// -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
+
+void setup()
+{
+
+  delay(1000);
+
+  SERIAL_PORT.begin(115200); // Start the serial console
+  SERIAL_PORT.println(F("ICM-20948 Example"));
+
+#ifdef USE_SPI
+  SPI_PORT.begin();
+#else
+  WIRE_PORT.begin();
+  WIRE_PORT.setClock(400000);
+#endif
+
+  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+
+  bool initialized = false;
+  while (!initialized)
+  {
+
+    // Initialize the ICM-20948
+    // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
+#ifdef USE_SPI
+    myICM.begin(CS_PIN, SPI_PORT);
+#else
+    myICM.begin(WIRE_PORT, AD0_VAL);
+#endif
+
+    SERIAL_PORT.print(F("Initialization of the sensor returned: "));
+    SERIAL_PORT.println(myICM.statusString());
+    if (myICM.status != ICM_20948_Stat_Ok)
+    {
+      SERIAL_PORT.println(F("Trying again..."));
+      delay(500);
+    }
+    else
+    {
+      initialized = true;
+    }
+  }
+
+  SERIAL_PORT.println(F("Device connected."));
+
+  bool success = true; // Use success to show if the DMP configuration was successful
+
+  // Initialize the DMP. initializeDMP is a weak function. In this example we overwrite it to change the sample rate (see below)
+  success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok);
+
+  // DMP sensor options are defined in ICM_20948_DMP.h
+  //    INV_ICM20948_SENSOR_ACCELEROMETER               (16-bit accel)
+  //    INV_ICM20948_SENSOR_GYROSCOPE                   (16-bit gyro + 32-bit calibrated gyro)
+  //    INV_ICM20948_SENSOR_RAW_ACCELEROMETER           (16-bit accel)
+  //    INV_ICM20948_SENSOR_RAW_GYROSCOPE               (16-bit gyro + 32-bit calibrated gyro)
+  //    INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED (16-bit compass)
+  //    INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED      (16-bit gyro)
+  //    INV_ICM20948_SENSOR_STEP_DETECTOR               (Pedometer Step Detector)
+  //    INV_ICM20948_SENSOR_STEP_COUNTER                (Pedometer Step Detector)
+  //    INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR        (32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_ROTATION_VECTOR             (32-bit 9-axis quaternion + heading accuracy)
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR (32-bit Geomag RV + heading accuracy)
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD           (32-bit calibrated compass)
+  //    INV_ICM20948_SENSOR_GRAVITY                     (32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_LINEAR_ACCELERATION         (16-bit accel + 32-bit 6-axis quaternion)
+  //    INV_ICM20948_SENSOR_ORIENTATION                 (32-bit 9-axis quaternion + heading accuracy)
+
+  // Enable the DMP orientation sensor
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
+
+  // Enable any additional sensors / features
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
+
+  // Configuring DMP to output data at multiple ODRs:
+  // DMP is capable of outputting multiple sensor data at different rates to FIFO.
+  // Setting value can be calculated as follows:
+  // Value = (DMP running rate / ODR ) - 1
+  // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+
+  // Enable the FIFO
+  success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
+
+  // Enable the DMP
+  success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
+
+  // Reset DMP
+  success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
+
+  // Reset FIFO
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // Check success
+  if (success)
+  {
+    SERIAL_PORT.println(F("DMP enabled."));
+  }
+  else
+  {
+    SERIAL_PORT.println(F("Enable DMP failed!"));
+    SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
+    while (1)
+      ; // Do nothing more
+  }
+
+  // Read existing biases from EEPROM
+  if (!EEPROM.begin(128)) // Allocate 128 Bytes for EEPROM storage. ESP32 needs this.
+  {
+    SERIAL_PORT.println(F("EEPROM.begin failed! You will not be able to save the biases..."));
+  }
+
+  biasStore store;
+
+  EEPROM.get(0, store); // Read existing EEPROM, starting at address 0
+  if (isBiasStoreValid(&store))
+  {
+    SERIAL_PORT.println(F("Bias data in EEPROM is valid. Restoring it..."));
+    success &= (myICM.setBiasGyroX(store.biasGyroX) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasGyroY(store.biasGyroY) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasGyroZ(store.biasGyroZ) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasAccelX(store.biasAccelX) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasAccelY(store.biasAccelY) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasAccelZ(store.biasAccelZ) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasCPassX(store.biasCPassX) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasCPassY(store.biasCPassY) == ICM_20948_Stat_Ok);
+    success &= (myICM.setBiasCPassZ(store.biasCPassZ) == ICM_20948_Stat_Ok);
+
+    if (success)
+    {
+      SERIAL_PORT.println(F("Biases restored."));
+      printBiases(&store);
+    }
+    else
+      SERIAL_PORT.println(F("Bias restore failed!"));
+  }
+
+  SERIAL_PORT.println(F("The biases will be saved in two minutes."));
+  SERIAL_PORT.println(F("Before then:"));
+  SERIAL_PORT.println(F("* Rotate the sensor around all three axes"));
+  SERIAL_PORT.println(F("* Hold the sensor stationary in all six orientations for a few seconds"));
+}
+
+void loop()
+{
+  static unsigned long startTime = millis(); // Save the biases when the code has been running for two minutes
+  static bool biasesStored = false;
+  
+  // Read any DMP data waiting in the FIFO
+  // Note:
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available.
+  //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
+  icm_20948_DMP_data_t data;
+  myICM.readDMPdataFromFIFO(&data);
+
+  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available?
+  {
+    //SERIAL_PORT.print(F("Received data! Header: 0x")); // Print the header in HEX so we can see what data is arriving in the FIFO
+    //if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
+    //if ( data.header < 0x100) SERIAL_PORT.print( "0" );
+    //if ( data.header < 0x10) SERIAL_PORT.print( "0" );
+    //SERIAL_PORT.println( data.header, HEX );
+
+    if ((data.header & DMP_header_bitmap_Quat9) > 0) // We have asked for orientation data so we should receive Quat9
+    {
+      // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1.
+      // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.
+      // The quaternion data is scaled by 2^30.
+
+      //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy);
+
+      // Scale to +/- 1
+      double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
+      double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
+
+/*
+      SERIAL_PORT.print(F("Q1:"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(" Q2:"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(" Q3:"));
+      SERIAL_PORT.print(q3, 3);
+      SERIAL_PORT.print(F(" Accuracy:"));
+      SERIAL_PORT.println(data.Quat9.Data.Accuracy);
+*/
+      
+      double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
+      double q2sqr = q2 * q2;
+
+      // roll (x-axis rotation)
+      double t0 = +2.0 * (q0 * q1 + q2 * q3);
+      double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr);
+      double roll = atan2(t0, t1) * 180.0 / PI;
+
+      // pitch (y-axis rotation)
+      double t2 = +2.0 * (q0 * q2 - q3 * q1);
+      t2 = t2 > 1.0 ? 1.0 : t2;
+      t2 = t2 < -1.0 ? -1.0 : t2;
+      double pitch = asin(t2) * 180.0 / PI;
+
+      // yaw (z-axis rotation)
+      double t3 = +2.0 * (q0 * q3 + q1 * q2);
+      double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
+      double yaw = atan2(t3, t4) * 180.0 / PI;
+
+      SERIAL_PORT.print(F("Roll: "));
+      SERIAL_PORT.print(roll, 1);
+      SERIAL_PORT.print(F("\tPitch: "));
+      SERIAL_PORT.print(pitch, 1);
+      SERIAL_PORT.print(F("\tYaw: "));
+      SERIAL_PORT.println(yaw, 1);
+    }
+  }
+
+  if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay
+  {
+    if (!biasesStored) // Should we store the biases?
+    {
+      if (millis() > (startTime + 120000)) // Is it time to store the biases?
+      {
+        SERIAL_PORT.println(F("\r\n\r\n\r\nSaving bias data..."));
+
+        biasStore store;
+          
+        bool success = (myICM.getBiasGyroX(&store.biasGyroX) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasGyroY(&store.biasGyroY) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasGyroZ(&store.biasGyroZ) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasAccelX(&store.biasAccelX) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasAccelY(&store.biasAccelY) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasAccelZ(&store.biasAccelZ) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasCPassX(&store.biasCPassX) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasCPassY(&store.biasCPassY) == ICM_20948_Stat_Ok);
+        success &= (myICM.getBiasCPassZ(&store.biasCPassZ) == ICM_20948_Stat_Ok);
+
+        updateBiasStoreSum(&store);
+      
+        if (success)
+        {
+          biasesStored = true; // Only attempt this once
+        
+          EEPROM.put(0, store); // Write biases to EEPROM, starting at address 0
+          EEPROM.commit(); // ESP32/SAMD/STM32 needs this
+  
+          EEPROM.get(0, store); // Read existing EEPROM, starting at address 0
+          if (isBiasStoreValid(&store))
+          {
+            SERIAL_PORT.println(F("Biases stored."));
+            printBiases(&store);
+            SERIAL_PORT.println(F("\r\n\r\n\r\n"));
+          }
+          else
+            SERIAL_PORT.println(F("Bias store failed!\r\n\r\n\r\n"));
+        }
+        else
+        {
+          SERIAL_PORT.println(F("Bias read failed!\r\n\r\n\r\n"));
+        }
+      }
+    }
+      
+    delay(10);
+  }
+}

From c10e76bcdc8b94fb98a0fbe2f6ec5a707c65e955 Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 12:56:39 +0000
Subject: [PATCH 7/9] Correct multi-line comments - for ESP32 v2.0.5

---
 .../Example10_DMP_FastMultipleSensors.ino                   | 6 +++---
 .../Example11_DMP_Bias_Save_Restore_ESP32.ino               | 1 -
 examples/Arduino/Example1_Basics/Example1_Basics.ino        | 6 +++---
 examples/Arduino/Example2_Advanced/Example2_Advanced.ino    | 6 +++---
 .../Arduino/Example3_Interrupts/Example3_Interrupts.ino     | 6 +++---
 .../Arduino/Example4_WakeOnMotion/Example4_WakeOnMotion.ino | 6 +++---
 .../Example6_DMP_Quat9_Orientation.ino                      | 6 +++---
 .../Example7_DMP_Quat6_EulerAngles.ino                      | 6 +++---
 .../Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino | 6 +++---
 .../Example9_DMP_MultipleSensors.ino                        | 6 +++---
 10 files changed, 27 insertions(+), 28 deletions(-)

diff --git a/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino b/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino
index 467cc84..b77b5c0 100644
--- a/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino
+++ b/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino
@@ -34,9 +34,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino
index b229790..1daa489 100644
--- a/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino
+++ b/examples/Arduino/Example11_DMP_Bias_Save_Restore_ESP32/Example11_DMP_Bias_Save_Restore_ESP32.ino
@@ -36,7 +36,6 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-
 // The value of the last bit of the I2C address.
 // On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
 #define AD0_VAL 1
diff --git a/examples/Arduino/Example1_Basics/Example1_Basics.ino b/examples/Arduino/Example1_Basics/Example1_Basics.ino
index 9d871b7..10cdb09 100644
--- a/examples/Arduino/Example1_Basics/Example1_Basics.ino
+++ b/examples/Arduino/Example1_Basics/Example1_Basics.ino
@@ -19,9 +19,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example2_Advanced/Example2_Advanced.ino b/examples/Arduino/Example2_Advanced/Example2_Advanced.ino
index c00a9d5..c10bef9 100644
--- a/examples/Arduino/Example2_Advanced/Example2_Advanced.ino
+++ b/examples/Arduino/Example2_Advanced/Example2_Advanced.ino
@@ -20,9 +20,9 @@
 #define CS_PIN 2         // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example3_Interrupts/Example3_Interrupts.ino b/examples/Arduino/Example3_Interrupts/Example3_Interrupts.ino
index 16e132e..0ede8b6 100644
--- a/examples/Arduino/Example3_Interrupts/Example3_Interrupts.ino
+++ b/examples/Arduino/Example3_Interrupts/Example3_Interrupts.ino
@@ -28,9 +28,9 @@
 #define CS_PIN 2         // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example4_WakeOnMotion/Example4_WakeOnMotion.ino b/examples/Arduino/Example4_WakeOnMotion/Example4_WakeOnMotion.ino
index d7fe982..6a1ca00 100644
--- a/examples/Arduino/Example4_WakeOnMotion/Example4_WakeOnMotion.ino
+++ b/examples/Arduino/Example4_WakeOnMotion/Example4_WakeOnMotion.ino
@@ -28,9 +28,9 @@
 #define CS_PIN 2         // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
index 0aa1bef..4c3b24a 100644
--- a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
+++ b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
@@ -36,9 +36,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
index f5daa1a..7cfccf1 100644
--- a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
+++ b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
@@ -36,9 +36,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
index b2d1819..0b833ce 100644
--- a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
+++ b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
@@ -34,9 +34,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
diff --git a/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino
index 0debeae..60d780d 100644
--- a/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino
+++ b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino
@@ -34,9 +34,9 @@
 #define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
 #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
-#define AD0_VAL 1      // The value of the last bit of the I2C address.                \
-                       // On the SparkFun 9DoF IMU breakout the default is 1, and when \
-                       // the ADR jumper is closed the value becomes 0
+// The value of the last bit of the I2C address.
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
 
 #ifdef USE_SPI
 ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object

From 4dbd88c58782689f63d948a7878323aac1503d43 Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 12:58:10 +0000
Subject: [PATCH 8/9] v1.2.10

---
 library.properties | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/library.properties b/library.properties
index 259ca29..e1c5d82 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library
-version=1.2.9
+version=1.2.10
 author=SparkFun Electronics <techsupport@sparkfun.com>
 maintainer=SparkFun Electronics <sparkfun.com>
 sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™).

From 368aa6b0701d574f1ceb763f81a62c2302393c0a Mon Sep 17 00:00:00 2001
From: Paul <pc235603@gmail.com>
Date: Mon, 14 Nov 2022 13:05:25 +0000
Subject: [PATCH 9/9] Update keywords.txt

---
 keywords.txt | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/keywords.txt b/keywords.txt
index 7015cbe..1912684 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -107,6 +107,15 @@ setBiasAccelZ	KEYWORD2
 setBiasCPassX	KEYWORD2
 setBiasCPassY	KEYWORD2
 setBiasCPassZ	KEYWORD2
+getBiasGyroX	KEYWORD2
+getBiasGyroY	KEYWORD2
+getBiasGyroZ	KEYWORD2
+getBiasAccelX	KEYWORD2
+getBiasAccelY	KEYWORD2
+getBiasAccelZ	KEYWORD2
+getBiasCPassX	KEYWORD2
+getBiasCPassY	KEYWORD2
+getBiasCPassZ	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)