Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Create Example11_DMP_Gravity_Oriented_Accelerometer.ino #73

Open
wants to merge 1 commit into
base: release_candidate
Choose a base branch
from
+597 −0
Open

Create Example11_DMP_Gravity_Oriented_Accelerometer.ino #73

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Create Example11_DMP_Gravity_Oriented_Accelerometer.ino 
As discussed with @PaulZC, this file rotates the raw accelerometer data using the Quat6 quaternion. An intialisation finds "down" and this is stored to allow the sensor to orient so that reported acceleration values are Up/Down, "N/S" and "E/W" in the external world frame.

I've set the sensor scale to 16g; if these values are exceeded then the quaternion loses it's lock. There is a routine that can be enabled to continuously correct the sensor to re-find gravity, but this is disabled by default.

I wrote this using an openlog artermis but it should be device independent  - any issues, please let me know!

Jon
@higherstateofawkwardness
higherstateofawkwardness authored Jun 11, 2021

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
GPG key ID: 4AEE18F83AFDEB23
Expired
Verified
Learn about vigilant mode
commit a4b5ebc39872d4d585fba4f391cef57ddf87fe98
597 changes: 597 additions & 0 deletions ...ple11_DMP_Gravity_Oriented_Accelerometer/Example11_DMP_Gravity_Oriented_Accelerometer.ino
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,597 @@
/****************************************************************
* Example11_DMP_Gravity_Oriented_Accelerometer.ino
* ICM 20948 Arduino Library Demo
* Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
*
* Jon Noble 19-May-21
* The following example uses QUAT6 for sensor orientation. An initialisation routine determines the gravity vector, and the accelerometer readings are then rotated onto a vector where down is always down (North-East-Up orientation).
* The maximum acceleration scale has been set as 16g; if this is exceeded then the sensor loses it's quaternion lock.
*
* Based on DMP example 10 by:
* Paul Clark, April 25th, 2021
*
* Based on original code by:
* Owen Lyke @ SparkFun Electronics
* Original Creation Date: April 17 2019
*
* ** 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 44 // 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

#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


double quat_x, quat_y,quat_z; // Quaternion corrected acceleration values
double g_x = 0, g_y = 0, g_z = -1000; // Vector used to store the gravity values and rotate the subsequent acceleration onto.
double abs_x, abs_y, abs_z; // Absolute orientation vector

void setup()
{
SERIAL_PORT.begin(115200); // Start the serial console
SERIAL_PORT.println(F("ICM-20948 Example"));

delay(100);

while (SERIAL_PORT.available()) // Make sure the serial RX buffer is empty
SERIAL_PORT.read();

SERIAL_PORT.println(F("Press any key to continue..."));

while (!SERIAL_PORT.available()) // Wait for the user to press a key (send any serial character)
;

#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 Linear Acceleration sensor (Quat6 + accel)
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_LINEAR_ACCELERATION) == ICM_20948_Stat_Ok);
//success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ROTATION_VECTOR) == ICM_20948_Stat_Ok);

// Enable 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 225Hz ODR rate when DMP is running at 225Hz, value = (225/225) - 1 = 0.
//success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to 225Hz
//success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to 225Hz

// For correct sensor fusion of the quaternion and the acceleration in this example, we need them to be running at the same rate.
// Winding the data rate up too far results in instability in the DMP.
int fusionrate = 4;
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, fusionrate) == ICM_20948_Stat_Ok); // Set to 225Hz
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, fusionrate) == ICM_20948_Stat_Ok); // Set to 225Hz

// 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
}

findGravity();

}

void loop()
{
int moreData = readIMUDMP();

if (moreData >=0) //function returns -1 if no data was read.
{

//Quat 6 sensor range set to 16g - if this is exceeded, then the quarternion will lose it's lock on which way is down. So slowly update gravity with the latest readings?
bool updategrav = false;

if (updategrav and (sqrt(quat_x*quat_x+quat_y*quat_y+quat_z*quat_z) < 1080)) // We can only update the gravity vector when the sensor is relatively stationary.
{
float ema = 0.01; // Use exponential moving average.
g_x = g_x*(1-ema)+ quat_x*ema;
g_y = g_y*(1-ema)+ quat_y*ema;
g_z = g_z*(1-ema)+ quat_z*ema;
}

bool fulloutput = false; //True outputs full descriptors, false outputs comma-separated values for plotting.
if (fulloutput)
{
SERIAL_PORT.print(F(" Accel: North:"));
SERIAL_PORT.print(abs_x);
SERIAL_PORT.print(F(" East:"));
SERIAL_PORT.print(abs_y);
SERIAL_PORT.print(F(" Down:"));
SERIAL_PORT.println(abs_z);
} else {
SERIAL_PORT.print(abs_x);
SERIAL_PORT.print(F(" ,"));
SERIAL_PORT.print(abs_y);
SERIAL_PORT.print(F(" ,"));
SERIAL_PORT.println(abs_z);
}
}

if(moreData <= 0){
//Wait if there is no more data available
delay(1);
}
}



int readIMUDMP() // return a value depending on whether there is valid data or not.
{
// 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_FIFONoDataAvail)
{
return -1;
}

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_Quat6) > 0)||((data.header & DMP_header_bitmap_Accel) > 0)) // Check for simulataneous quaternion and accel data
{
// 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("Quat6 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat6.Data.Q1, data.Quat6.Data.Q2, data.Quat6.Data.Q3);

// Scale to +/- 1
double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
double q0 = sqrt( 1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data
float acc_y = (float)data.Raw_Accel.Data.Y;
float acc_z = (float)data.Raw_Accel.Data.Z;


/* Uncomment the following for raw data output from the Quaternion and the Accelerometer
SERIAL_PORT.print(F("Q0:"));
SERIAL_PORT.print(q0, 3);
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(" Accel: X:"));
SERIAL_PORT.print(acc_x);
SERIAL_PORT.print(F(" Y:"));
SERIAL_PORT.print(acc_y);
SERIAL_PORT.print(F(" Z:"));
SERIAL_PORT.println(acc_z);
*/

// Quaternion is initialised as q1=q2=q3=0. Perform Quaternion rotation of raw acceleration values into the initial sensor rotation frame:

quat_x = acc_x*(1-2*q2*q2-2*q3*q3)+acc_y*2*(q1*q2-q0*q3)+acc_z*2*(q1*q3+q0*q2);
quat_y = acc_x*2*(q1*q2+q0*q3)+acc_y*(1-2*q1*q1-2*q3*q3)+acc_z*2*(q2*q3-q0*q1);
quat_z = acc_x*2*(q1*q3-q0*q2)+acc_y*2*(q2*q3+q0*q1)+acc_z*(1-2*q1*q1-2*q2*q2);

// Re-orient the sensor so that gravity is down. See: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
// Basically, we want the quaternion rotation to rotate the whole system so that the gravity vector (x,y,z) maps onto the unit vector (0,0,-1)
// The cross-product of the gravity and the "down" vector is (-y,x,0) and the dot-product is (-z).
// The rotation quaternion is therefore given by (q0,q1,q2,q3) = (sqrt(g_x*g_x+g_y*g_y+g_z*g_z)-g_z , -g_y , g_x , 0); although this needs normalising before application.

// Re-use the previous variables from Quat rotation - this might not be best practice
q0 = sqrt(g_x*g_x+g_y*g_y+g_z*g_z)-g_z;
q1 = -g_y;
q2 = g_x;
q3 = 0;
double quatMag = sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0 /= quatMag;
q1 /= quatMag;
q2 /= quatMag;
q3 /= quatMag;

abs_x = quat_x*(1-2*q2*q2-2*q3*q3) + quat_y*2*(q1*q2-q0*q3) + quat_z*2*(q1*q3+q0*q2);
abs_y = quat_x*2*(q1*q2+q0*q3) + quat_y*(1-2*q1*q1-2*q3*q3) + quat_z*2*(q2*q3-q0*q1);
abs_z = quat_x*2*(q1*q3-q0*q2) + quat_y*2*(q2*q3+q0*q1) + quat_z*(1-2*q1*q1-2*q2*q2);


bool subtractG = true; // Do you want to subtract the gravity reading (true), or just orient the sensor (false)
if (subtractG)
{
abs_z += sqrt(g_x*g_x+g_y*g_y+g_z*g_z);
}
}


}

if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay
{
return 0; // No more data
}
else
{
return 1; // More data
}
}


void findGravity()
{
Serial.println("Calibrating gravity vector - keep the sensor still or this might give inaccurate results");
//Do exponential moving average over a certain time period and wait until vector settles at around 1g. Takes around 8 seconds.
double totalAcc = sqrt(g_x*g_x + g_y*g_y+ g_z*g_z);
float ema = 0.01;
// Average the first xxx readings.DMP seems to take a few seconds to settle down anyhow.
int counter = 750;
int i = 0;
double grav_x = 0, grav_y = 0, grav_z = 0;

while (i < counter)
{
int moreData = readIMUDMP();
if (moreData >=0) //function returns -1 if no data was read.
{
grav_x += quat_x/counter;
grav_y += quat_y/counter;
grav_z += quat_z/counter;
i += 1;
}

if(moreData <=0){
delay(1);
}

}
g_x = grav_x;
g_y = grav_y;
g_z = grav_z;

}



// initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate
ICM_20948_Status_e ICM_20948::initializeDMP(void)
{
// The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
// sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".

ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful
ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok;

// Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer
// to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read
// nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit.
// Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit.
//
// But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that,
// when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved
// RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here...
// The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04.
// We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this...
//
// So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are:
// 0: use I2C_SLV0
// MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted)
// AK09916_REG_RSV2: we start reading here (0x03). Secret sauce...
// 10: we read 10 bytes each cycle
// true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them)
// true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate
// false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value)
// true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm)
// true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm)
result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result;
//
// We also need to set up I2C_SLV1 to do the Single Measurement triggering:
// 1: use I2C_SLV1
// MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted)
// AK09916_REG_CNTL2: we start writing here (0x31)
// 1: not sure why, but the write does not happen if this is set to zero
// false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte
// true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear
// false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value)
// false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit
// false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit
// AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample
result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result;

// Set the I2C Master ODR configuration
// It is not clear why we need to do this... But it appears to be essential! From the datasheet:
// "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled.
// ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) )
// When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR.
// If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR."
// Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)?
// You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case.
result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3
uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register

// Configure clock source through PWR_MGMT_1
// ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails

// Enable accel and gyro sensors through PWR_MGMT_2
// Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6
result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register

// Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
result = setSampleMode((ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result;

// Disable the FIFO
result = enableFIFO(false); if (result > worstResult) worstResult = result;

// Disable the DMP
result = enableDMP(false); if (result > worstResult) worstResult = result;

// Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
// Set Accel FSR (Full scale range) to 16g through ACCEL_CONFIG
ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
myFSS.a = gpm16; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
// gpm2
// gpm4
// gpm8
// gpm16
myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
// dps250
// dps500
// dps1000
// dps2000
result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result;

// Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
// If we see this interrupt, we'll need to reset the FIFO
//result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs

// Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2
// Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
uint8_t zero = 0;
result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result;
// Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result;

// Turn off data ready interrupt through INT_ENABLE_1
result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result;

// Reset FIFO through FIFO_RST
result = resetFIFO(); if (result > worstResult) worstResult = result;

// Set gyro sample rate divider with GYRO_SMPLRT_DIV
// Set accel sample rate divider with ACCEL_SMPLRT_DIV_2
ICM_20948_smplrt_t mySmplrt;
mySmplrt.g = 4; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 4 = 220Hz
mySmplrt.a = 4; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 4 = 225Hz
result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result;

// Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS

// Now load the DMP firmware
result = loadDMPFirmware(); if (result > worstResult) worstResult = result;

// Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS

// Set the Hardware Fix Disable register to 0x48
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
uint8_t fix = 0x48;
result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result;

// Set the Single FIFO Priority Select register to 0xE4
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
uint8_t fifoPrio = 0xE4;
result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result;

// Configure Accel scaling to DMP
// The DMP scales accel raw data internally to align 1g as 2^25
// In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g, 0x08000000 for 16g
const unsigned char accScale[4] = {0x08, 0x00, 0x00, 0x00};
result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register
// In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g, 0x08000000 for 16g
const unsigned char accScale2[4] = {0x00, 0x08, 0x00, 0x00};
result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register

// Configure Compass mount matrix and scale to DMP
// The mount matrix write to DMP register is used to align the compass axes with accel/gyro.
// This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30.
// Each compass axis will be converted as below:
// X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02
// Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12
// Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
// The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU.
// 2^30 / 6.66666 = 161061273 = 0x9999999
const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example
const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example
result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;

// Configure the B2S Mounting Matrix
const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;

// Configure the DMP Gyro Scaling Factor
// @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where
// 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ...
// 10=102.2727Hz sample rate, ... etc.
// @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps
result = setGyroSF(4, 3); if (result > worstResult) worstResult = result; // 0 = 225Hz (see above), 3 = 2000dps (see above)

// Configure the Gyro full scale
// 2000dps : 2^28
// 1000dps : 2^27
// 500dps : 2^26
// 250dps : 2^25
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result;

// Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
//const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz
result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result;

// Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
//const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz
result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result;

// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
//const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz
result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result;

// Configure the Accel Cal Rate
const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result;

// Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz.
// Let's set the Compass Time Buffer to 69 (Hz).
const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz
result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result;

// Enable DMP interrupt
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
//result = intEnableDMP(true); if (result > worstResult) worstResult = result;

return worstResult;
}