Skip to content

Commit 4f4c965

Browse files
PaulZCPaulZC
committedFeb 26, 2021
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).
·
v1.3.0v1.2.0
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

+1-1
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

+36-27
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()

‎examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino

+329
Large diffs are not rendered by default.

‎examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino

+304
Large diffs are not rendered by default.

‎src/ICM_20948.cpp

+12-12
Original file line numberDiff line numberDiff line change
@@ -850,6 +850,16 @@ ICM_20948_Status_e ICM_20948::startupDefault(bool minimal)
850850
return status;
851851
}
852852

853+
retval = startupMagnetometer();
854+
if (retval != ICM_20948_Stat_Ok)
855+
{
856+
debugPrint(F("ICM_20948::startupDefault: startupMagnetometer returned: "));
857+
debugPrintStatus(retval);
858+
debugPrintln(F(""));
859+
status = retval;
860+
return status;
861+
}
862+
853863
if (minimal) // Return now if minimal is true
854864
{
855865
debugPrintln(F("ICM_20948::startupDefault: minimal startup complete!"));
@@ -912,16 +922,6 @@ ICM_20948_Status_e ICM_20948::startupDefault(bool minimal)
912922
return status;
913923
}
914924

915-
retval = startupMagnetometer();
916-
if (retval != ICM_20948_Stat_Ok)
917-
{
918-
debugPrint(F("ICM_20948::startupDefault: startupMagnetometer returned: "));
919-
debugPrintStatus(retval);
920-
debugPrintln(F(""));
921-
status = retval;
922-
return status;
923-
}
924-
925925
return status;
926926
}
927927

@@ -977,9 +977,9 @@ ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count)
977977
return status;
978978
}
979979

980-
ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data)
980+
ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data, uint8_t len)
981981
{
982-
status = ICM_20948_read_FIFO(&_device, data);
982+
status = ICM_20948_read_FIFO(&_device, data, len);
983983
return status;
984984
}
985985

‎src/ICM_20948.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -168,7 +168,7 @@ class ICM_20948
168168
ICM_20948_Status_e resetFIFO(void);
169169
ICM_20948_Status_e setFIFOmode(bool snapshot = false); // Default to Stream (non-Snapshot) mode
170170
ICM_20948_Status_e getFIFOcount(uint16_t *count);
171-
ICM_20948_Status_e readFIFO(uint8_t *data);
171+
ICM_20948_Status_e readFIFO(uint8_t *data, uint8_t len = 1);
172172

173173
//DMP
174174

‎src/util/ICM_20948_C.c

+153-129
Large diffs are not rendered by default.

‎src/util/ICM_20948_C.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -228,7 +228,7 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
228228
ICM_20948_Status_e ICM_20948_reset_FIFO(ICM_20948_Device_t *pdev);
229229
ICM_20948_Status_e ICM_20948_set_FIFO_mode(ICM_20948_Device_t *pdev, bool snapshot);
230230
ICM_20948_Status_e ICM_20948_get_FIFO_count(ICM_20948_Device_t *pdev, uint16_t *count);
231-
ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t *pdev, uint8_t *data);
231+
ICM_20948_Status_e ICM_20948_read_FIFO(ICM_20948_Device_t *pdev, uint8_t *data, uint8_t len);
232232

233233
// DMP
234234

‎src/util/ICM_20948_DMP.h

+14-5
Original file line numberDiff line numberDiff line change
@@ -573,17 +573,18 @@ typedef struct // DMP Secondary On/Off data
573573
#define icm_20948_DMP_PQuat6_Bytes 6
574574
#define icm_20948_DMP_Geomag_Bytes 14
575575
#define icm_20948_DMP_Pressure_Bytes 6
576-
#define icm_20948_DMP_Gyro_Calibr_Bytes 12 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes does not exist?
576+
#define icm_20948_DMP_Gyro_Calibr_Bytes 12 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes is not supported?
577577
#define icm_20948_DMP_Compass_Calibr_Bytes 12
578578
#define icm_20948_DMP_Step_Detector_Bytes 4 // See note above
579579
#define icm_20948_DMP_Accel_Accuracy_Bytes 2
580580
#define icm_20948_DMP_Gyro_Accuracy_Bytes 2
581581
#define icm_20948_DMP_Compass_Accuracy_Bytes 2
582-
#define icm_20948_DMP_Fsync_Detection_Bytes 2 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes does not exist?
582+
#define icm_20948_DMP_Fsync_Detection_Bytes 2 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes is not supported?
583583
#define icm_20948_DMP_Pickup_Bytes 2
584584
#define icm_20948_DMP_Activity_Recognition_Bytes 6
585585
#define icm_20948_DMP_Secondary_On_Off_Bytes 2
586586
#define icm_20948_DMP_Footer_Bytes 2
587+
#define icm_20948_DMP_Maximum_Bytes 14 // The most bytes we will attempt to read from the FIFO in one go
587588

588589
// ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t
589590
const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] =
@@ -596,7 +597,11 @@ const int DMP_Quat6_Byte_Ordering[icm_20948_DMP_Quat6_Bytes] =
596597
};
597598
const int DMP_PQuat6_Byte_Ordering[icm_20948_DMP_PQuat6_Bytes] =
598599
{
599-
1,0,3,2,5,4 // Also used for Raw_Accel, Raw_Gyro, Compass
600+
1,0,3,2,5,4 // Also used for Raw_Accel, Compass
601+
};
602+
const int DMP_Raw_Gyro_Byte_Ordering[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes] =
603+
{
604+
1,0,3,2,5,4,7,6,9,8,11,10
600605
};
601606
const int DMP_Activity_Recognition_Byte_Ordering[icm_20948_DMP_Activity_Recognition_Bytes] =
602607
{
@@ -623,14 +628,17 @@ typedef struct
623628
} Raw_Accel;
624629
union
625630
{
626-
uint8_t Bytes[icm_20948_DMP_Raw_Gyro_Bytes];
631+
uint8_t Bytes[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes];
627632
struct
628633
{
629634
int16_t X;
630635
int16_t Y;
631636
int16_t Z;
637+
int16_t BiasX;
638+
int16_t BiasY;
639+
int16_t BiasZ;
632640
} Data;
633-
} Raw_Gyro; // Note: may also include 3*16 gyro bias?
641+
} Raw_Gyro;
634642
union
635643
{
636644
uint8_t Bytes[icm_20948_DMP_Compass_Bytes];
@@ -755,6 +763,7 @@ typedef struct
755763
uint8_t Bytes[icm_20948_DMP_Secondary_On_Off_Bytes];
756764
icm_20948_DMP_Secondary_On_Off_t Sensors;
757765
} Secondary_On_Off;
766+
uint16_t Footer; // Gyro count?
758767
} icm_20948_DMP_data_t;
759768

760769
#ifdef __cplusplus

‎src/util/ICM_20948_REGISTERS.h

-4
Original file line numberDiff line numberDiff line change
@@ -483,10 +483,6 @@ typedef struct{
483483
uint8_t FIFO_COUNTL;
484484
}ICM_20948_FIFO_COUNTL_t;
485485

486-
typedef struct{
487-
uint8_t FIFO_R_W; // Reading from or writing to this register actually reads from or writes to the FIFO
488-
}ICM_20948_FIFO_R_W_t;
489-
490486
typedef struct{
491487
uint8_t RAW_DATA_RDY : 4;
492488
uint8_t reserved_0 : 3;

0 commit comments

Comments
 (0)
Please sign in to comment.