Skip to content

Commit 4f4c965

Browse files
PaulZCPaulZC
committed
DMP Improvements
DMP functionality now seems good; Euler angles are well-behaved. Ensured the magnetometer is started correctly. Corrected the compass matrix values in the DMP examples. Added the B2S matrix in the DMP examples. Added new examples for Quat6 (with Euler angles) and RawAccel. Improved readFIFO so it can read multiple bytes consecutively (substantially improving the I2C throughput).
1 parent 89cd1a2 commit 4f4c965

File tree

10 files changed

+851
-180
lines changed

10 files changed

+851
-180
lines changed

examples/Arduino/Example6_DualSPITest/Example6_DualSPITest.ino renamed to examples/Arduino/Example5_DualSPITest/Example5_DualSPITest.ino

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/****************************************************************
2-
* Example6_DualSPITest.ino
2+
* Example5_DualSPITest.ino
33
* ICM 20948 Arduino Library Demo
44
* Use the default configuration to stream 9-axis IMU data on two IMUs over SPI
55
* Owen Lyke @ SparkFun Electronics

examples/Arduino/Example5_DMP_Quat9_Orientation/Example5_DMP_Quat9_Orientation.ino renamed to examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino

Lines changed: 36 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/****************************************************************
2-
* Example5_DMP_Quat9_Orientation.ino
2+
* Example6_DMP_Quat9_Orientation.ino
33
* ICM 20948 Arduino Library Demo
44
* Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
55
* Paul Clark, February 15th 2021
@@ -83,7 +83,7 @@ void setup() {
8383
SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
8484
SERIAL_PORT.println( myICM.statusString() );
8585
if( myICM.status != ICM_20948_Stat_Ok ){
86-
SERIAL_PORT.println( "Trying again..." );
86+
SERIAL_PORT.println( F("Trying again...") );
8787
delay(500);
8888
}else{
8989
initialized = true;
@@ -106,8 +106,14 @@ void setup() {
106106
uint8_t zero = 0;
107107
success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &zero, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register
108108

109-
// Configure Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
110-
success &= (myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
109+
// Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
110+
success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
111+
112+
// Disable the FIFO
113+
success &= (myICM.enableFIFO(false) == ICM_20948_Stat_Ok);
114+
115+
// Disable the DMP
116+
success &= (myICM.enableDMP(false) == ICM_20948_Stat_Ok);
111117

112118
// Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
113119
// Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG
@@ -172,11 +178,9 @@ void setup() {
172178
// X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02
173179
// Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12
174180
// Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
175-
// Magnetometer full scale is +/- 4900uT so _I think_ we need to multiply by 2^30 / 4900 = 0x000357FA
176-
// The magnetometer Y and Z axes are reversed compared to the accelerometer so we'll invert those
177181
const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
178-
const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
179-
const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
182+
const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example
183+
const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example
180184
success &= (myICM.writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
181185
success &= (myICM.writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
182186
success &= (myICM.writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
@@ -187,22 +191,27 @@ void setup() {
187191
success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
188192
success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
189193

190-
// // Configure the biases
191-
// // The biases are 32-bits in chip frame in hardware unit scaled by:
192-
// // 2^12 (FSR 4g) for accel, 2^15 for gyro, in uT scaled by 2^16 for compass.
193-
// const unsigned char accelBiasOne[4] = {0x00, 0x00, 0x10, 0x00};
194-
// const unsigned char gyroBiasOne[4] = {0x00, 0x00, 0x80, 0x00};
195-
// const unsigned char compassBiasOne[4] = {0x00, 0x01, 0x00, 0x00};
196-
// success &= (myICM.writeDMPmems(GYRO_BIAS_X, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
197-
// success &= (myICM.writeDMPmems(GYRO_BIAS_Y, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
198-
// success &= (myICM.writeDMPmems(GYRO_BIAS_Z, 4, &gyroBiasOne[0]) == ICM_20948_Stat_Ok);
199-
// success &= (myICM.writeDMPmems(ACCEL_BIAS_X, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
200-
// success &= (myICM.writeDMPmems(ACCEL_BIAS_Y, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
201-
// success &= (myICM.writeDMPmems(ACCEL_BIAS_Z, 4, &accelBiasOne[0]) == ICM_20948_Stat_Ok);
202-
// success &= (myICM.writeDMPmems(CPASS_BIAS_X, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
203-
// success &= (myICM.writeDMPmems(CPASS_BIAS_Y, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
204-
// success &= (myICM.writeDMPmems(CPASS_BIAS_Z, 4, &compassBiasOne[0]) == ICM_20948_Stat_Ok);
205-
194+
// Configure the B2S Mounting Matrix
195+
const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
196+
const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
197+
success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
198+
success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
199+
success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
200+
success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
201+
success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
202+
success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
203+
success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
204+
success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
205+
success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
206+
207+
// Configure the Gyro scaling factor
208+
const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example
209+
success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok);
210+
211+
// Configure the Gyro full scale
212+
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
213+
success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
214+
206215
// Enable DMP interrupt
207216
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
208217
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
@@ -246,12 +255,12 @@ void setup() {
246255

247256
// Check success
248257
if( success )
249-
SERIAL_PORT.println("DMP enabled!");
258+
SERIAL_PORT.println(F("DMP enabled!"));
250259
else
251260
{
252-
SERIAL_PORT.println("Enable DMP failed!");
261+
SERIAL_PORT.println(F("Enable DMP failed!"));
262+
SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
253263
}
254-
255264
}
256265

257266
void loop()

0 commit comments

Comments
 (0)