Skip to content

Commit 87233c0

Browse files
PaulZCPaulZC
committed
Corrected the DMP data types and byte ordering
1 parent 0051282 commit 87233c0

File tree

5 files changed

+440
-452
lines changed

5 files changed

+440
-452
lines changed

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

Lines changed: 66 additions & 131 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,14 @@
11
/****************************************************************
2-
* Example5_DMP.ino
2+
* Example5_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 9th 2021
66
* Based on original code by:
77
* Owen Lyke @ SparkFun Electronics
88
* Original Creation Date: April 17 2019
99
*
10-
* ** This example is based on the InvenSense Application Note "Programming Sequence for DMP Hardware Functions".
11-
* ** We are grateful to InvenSense for providing this.
10+
* ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
11+
* ** We are grateful to InvenSense for sharing this with us.
1212
*
1313
* ** Important note: by default the DMP functionality is disabled in the library
1414
* ** as the DMP firmware takes up 14290 Bytes of program memory.
@@ -92,19 +92,20 @@ void setup() {
9292
SERIAL_PORT.println("Device connected!");
9393

9494
// The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
95+
// sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
9596

9697
bool success = true; // Use success to show if the configuration was successful
9798

9899
// Configure clock source through PWR_MGMT_1
99-
// Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
100+
// ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
100101
success &= (myICM.setClockSource(ICM_20948_Clock_Auto) == ICM_20948_Stat_Ok); // This is shorthand: success will be set to false if setClockSource fails
101102

102103
// Enable accel and gyro sensors through PWR_MGMT_2
103104
// Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
104105
uint8_t zero = 0;
105106
success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &zero, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register
106107

107-
// Configure Gyro/Accel in Low power mode with LP_CONFIG
108+
// Configure Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
108109
success &= (myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
109110

110111
// Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
@@ -123,9 +124,10 @@ void setup() {
123124
success &= (myICM.setFullScale( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS ) == ICM_20948_Stat_Ok);
124125

125126
// Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
127+
// If we see this interrupt, we'll need to reset the FIFO
126128
//success &= (myICM.intEnableOverflowFIFO( 0x1F ) == ICM_20948_Stat_Ok); // Enable the interrupt on all FIFOs
127129

128-
// Turn off what goes into the FIFO through FIFO_EN, FIFO_EN_2
130+
// Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2
129131
// Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
130132
success &= (myICM.write(AGB0_REG_FIFO_EN_1, &zero, 1) == ICM_20948_Stat_Ok);
131133
// Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
@@ -184,26 +186,48 @@ void setup() {
184186
success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
185187
success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
186188

189+
// Enable DMP interrupt
190+
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
191+
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
192+
193+
// DMP sensor options are defined in ICM_20948_DMP.h
194+
// INV_ICM20948_SENSOR_ACCELEROMETER
195+
// INV_ICM20948_SENSOR_GYROSCOPE
196+
// INV_ICM20948_SENSOR_RAW_ACCELEROMETER
197+
// INV_ICM20948_SENSOR_RAW_GYROSCOPE
198+
// INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED
199+
// INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED
200+
// INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON
201+
// INV_ICM20948_SENSOR_STEP_DETECTOR
202+
// INV_ICM20948_SENSOR_STEP_COUNTER
203+
// INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR
204+
// INV_ICM20948_SENSOR_ROTATION_VECTOR
205+
// INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR
206+
// INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD
207+
// INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION
208+
// INV_ICM20948_SENSOR_FLIP_PICKUP
209+
// INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR
210+
// INV_ICM20948_SENSOR_GRAVITY
211+
// INV_ICM20948_SENSOR_LINEAR_ACCELERATION
212+
// INV_ICM20948_SENSOR_ORIENTATION
213+
214+
// Enable the DMP orientation sensor
215+
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
216+
217+
// Set the DMP orientation sensor rate to 25Hz
218+
success &= (myICM.setDMPODRrate(INV_ICM20948_SENSOR_ORIENTATION, 25) == ICM_20948_Stat_Ok);
219+
187220
// Enable the FIFO
188221
success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
189222

190-
// Reset FIFO
191-
success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
192-
193223
// Enable the DMP
194224
success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
195225

196226
// Reset DMP
197227
success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
198228

199-
// Enable DMP interrupt
200-
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
201-
202-
// Enable the DMP orientation sensor
203-
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
204-
205-
// Set the DMP orientation sensor rate to 25Hz
206-
success &= (myICM.setDMPODRrate(INV_ICM20948_SENSOR_ORIENTATION, 25) == ICM_20948_Stat_Ok);
229+
// Reset FIFO
230+
success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
207231

208232
// Check success
209233
if( success )
@@ -218,145 +242,56 @@ void setup() {
218242

219243
void loop()
220244
{
245+
// Read any data waiting in the FIFO
246+
// Note:
247+
// readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data or incomplete data is available.
248+
// If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
249+
// readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
250+
// readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
221251
icm_20948_DMP_data_t data;
222252
myICM.readDMPdataFromFIFO(&data);
223-
if( myICM.status == ICM_20948_Stat_Ok )
253+
254+
if(( myICM.status == ICM_20948_Stat_Ok ) || ( myICM.status == ICM_20948_Stat_FIFOMoreDataAvail )) // Was valid data available?
224255
{
225-
SERIAL_PORT.print(F("Received data! Header: 0x"));
256+
SERIAL_PORT.print(F("Received data! Header: 0x")); // Print the header in HEX
226257
if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
227258
if ( data.header < 0x100) SERIAL_PORT.print( "0" );
228259
if ( data.header < 0x10) SERIAL_PORT.print( "0" );
229260
SERIAL_PORT.println( data.header, HEX );
230-
if ( (data.header | DMP_header_bitmap_Quat9) > 0 ) // We have asked for orientation data so we should receive Quat9
261+
262+
if ( (data.header & DMP_header_bitmap_Quat9) > 0 ) // We have asked for orientation data so we should receive Quat9
231263
{
232-
SERIAL_PORT.print(F("Quat9 data is: 0x"));
233-
for (int i = 0; i < 14; i++) // Quat9 data is 14 bytes long
234-
{
235-
if ( data.Quat9[i] < 0x10) SERIAL_PORT.print( "0" ); // Pad the zero
236-
SERIAL_PORT.print( data.Quat9[i], HEX );
237-
}
238-
SERIAL_PORT.println();
264+
// Q0 value is computed from this equation: Q20 + Q21 + Q22 + Q23 = 1.
265+
// In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.
266+
// The quaternion data is scaled by 2^30.
267+
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);
239268
}
240269
}
241-
else if ( myICM.status != ICM_20948_Stat_FIFONoDataAvail )
270+
else if ( myICM.status != ICM_20948_Stat_FIFONoDataAvail ) // Check for an error - but ignore "no data available"
242271
{
243-
SERIAL_PORT.print(F("readDMPdataFromFIFO failed! Status is: "));
272+
SERIAL_PORT.print(F("readDMPdataFromFIFO failed! Status is: ")); // We got an error - so print it
244273
SERIAL_PORT.print( myICM.status );
245274
SERIAL_PORT.print(" : ");
246275
SERIAL_PORT.println( myICM.statusString() );
247-
SERIAL_PORT.print(F("Header is: 0x"));
276+
277+
SERIAL_PORT.print(F("Header is: 0x")); // Print the header so we can check it manually
248278
if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
249279
if ( data.header < 0x100) SERIAL_PORT.print( "0" );
250280
if ( data.header < 0x10) SERIAL_PORT.print( "0" );
251281
SERIAL_PORT.println( data.header, HEX );
252-
if ( data.header == DMP_header_bitmap_Header2 )
282+
283+
if ( data.header2 > 0 )
253284
{
254-
SERIAL_PORT.print(F("Header2 is: 0x"));
285+
SERIAL_PORT.print(F("Header2 is: 0x")); // Print header2 so we can check it manually
255286
if ( data.header2 < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
256287
if ( data.header2 < 0x100) SERIAL_PORT.print( "0" );
257288
if ( data.header2 < 0x10) SERIAL_PORT.print( "0" );
258289
SERIAL_PORT.println( data.header2, HEX );
259290
}
260291
}
261292

262-
delay(10);
263-
}
264-
265-
266-
// Below here are some helper functions to print the data nicely!
267-
268-
void printPaddedInt16b( int16_t val ){
269-
if(val > 0){
270-
SERIAL_PORT.print(" ");
271-
if(val < 10000){ SERIAL_PORT.print("0"); }
272-
if(val < 1000 ){ SERIAL_PORT.print("0"); }
273-
if(val < 100 ){ SERIAL_PORT.print("0"); }
274-
if(val < 10 ){ SERIAL_PORT.print("0"); }
275-
}else{
276-
SERIAL_PORT.print("-");
277-
if(abs(val) < 10000){ SERIAL_PORT.print("0"); }
278-
if(abs(val) < 1000 ){ SERIAL_PORT.print("0"); }
279-
if(abs(val) < 100 ){ SERIAL_PORT.print("0"); }
280-
if(abs(val) < 10 ){ SERIAL_PORT.print("0"); }
281-
}
282-
SERIAL_PORT.print(abs(val));
283-
}
284-
285-
void printRawAGMT( ICM_20948_AGMT_t agmt){
286-
SERIAL_PORT.print("RAW. Acc [ ");
287-
printPaddedInt16b( agmt.acc.axes.x );
288-
SERIAL_PORT.print(", ");
289-
printPaddedInt16b( agmt.acc.axes.y );
290-
SERIAL_PORT.print(", ");
291-
printPaddedInt16b( agmt.acc.axes.z );
292-
SERIAL_PORT.print(" ], Gyr [ ");
293-
printPaddedInt16b( agmt.gyr.axes.x );
294-
SERIAL_PORT.print(", ");
295-
printPaddedInt16b( agmt.gyr.axes.y );
296-
SERIAL_PORT.print(", ");
297-
printPaddedInt16b( agmt.gyr.axes.z );
298-
SERIAL_PORT.print(" ], Mag [ ");
299-
printPaddedInt16b( agmt.mag.axes.x );
300-
SERIAL_PORT.print(", ");
301-
printPaddedInt16b( agmt.mag.axes.y );
302-
SERIAL_PORT.print(", ");
303-
printPaddedInt16b( agmt.mag.axes.z );
304-
SERIAL_PORT.print(" ], Tmp [ ");
305-
printPaddedInt16b( agmt.tmp.val );
306-
SERIAL_PORT.print(" ]");
307-
SERIAL_PORT.println();
308-
}
309-
310-
311-
void printFormattedFloat(float val, uint8_t leading, uint8_t decimals){
312-
float aval = abs(val);
313-
if(val < 0){
314-
SERIAL_PORT.print("-");
315-
}else{
316-
SERIAL_PORT.print(" ");
317-
}
318-
for( uint8_t indi = 0; indi < leading; indi++ ){
319-
uint32_t tenpow = 0;
320-
if( indi < (leading-1) ){
321-
tenpow = 1;
322-
}
323-
for(uint8_t c = 0; c < (leading-1-indi); c++){
324-
tenpow *= 10;
325-
}
326-
if( aval < tenpow){
327-
SERIAL_PORT.print("0");
328-
}else{
329-
break;
330-
}
331-
}
332-
if(val < 0){
333-
SERIAL_PORT.print(-val, decimals);
334-
}else{
335-
SERIAL_PORT.print(val, decimals);
293+
if ( myICM.status != ICM_20948_Stat_FIFOMoreDataAvail ) // If more data is available then we should read it right away - and not delay
294+
{
295+
delay(10);
336296
}
337297
}
338-
339-
void printScaledAGMT( ICM_20948_AGMT_t agmt){
340-
SERIAL_PORT.print("Scaled. Acc (mg) [ ");
341-
printFormattedFloat( myICM.accX(), 5, 2 );
342-
SERIAL_PORT.print(", ");
343-
printFormattedFloat( myICM.accY(), 5, 2 );
344-
SERIAL_PORT.print(", ");
345-
printFormattedFloat( myICM.accZ(), 5, 2 );
346-
SERIAL_PORT.print(" ], Gyr (DPS) [ ");
347-
printFormattedFloat( myICM.gyrX(), 5, 2 );
348-
SERIAL_PORT.print(", ");
349-
printFormattedFloat( myICM.gyrY(), 5, 2 );
350-
SERIAL_PORT.print(", ");
351-
printFormattedFloat( myICM.gyrZ(), 5, 2 );
352-
SERIAL_PORT.print(" ], Mag (uT) [ ");
353-
printFormattedFloat( myICM.magX(), 5, 2 );
354-
SERIAL_PORT.print(", ");
355-
printFormattedFloat( myICM.magY(), 5, 2 );
356-
SERIAL_PORT.print(", ");
357-
printFormattedFloat( myICM.magZ(), 5, 2 );
358-
SERIAL_PORT.print(" ], Tmp (C) [ ");
359-
printFormattedFloat( myICM.temp(), 5, 2 );
360-
SERIAL_PORT.print(" ]");
361-
SERIAL_PORT.println();
362-
}

‎src/ICM_20948.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -116,6 +116,9 @@ void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat)
116116
case ICM_20948_Stat_FIFONoDataAvail:
117117
debugPrint(F("No FIFO Data Available"));
118118
break;
119+
case ICM_20948_Stat_FIFOMoreDataAvail:
120+
debugPrint(F("More FIFO Data Available"));
121+
break;
119122
case ICM_20948_Stat_UnrecognisedDMPHeader:
120123
debugPrint(F("Unrecognised DMP Header"));
121124
break;
@@ -286,6 +289,9 @@ const char *ICM_20948::statusString(ICM_20948_Status_e stat)
286289
case ICM_20948_Stat_FIFONoDataAvail:
287290
return "No FIFO Data Available";
288291
break;
292+
case ICM_20948_Stat_FIFOMoreDataAvail:
293+
return "More FIFO Data Available";
294+
break;
289295
case ICM_20948_Stat_UnrecognisedDMPHeader:
290296
return "Unrecognised DMP Header";
291297
break;
@@ -1122,6 +1128,8 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
11221128
#endif
11231129

11241130
_device._firmware_loaded = false; // Initialize _firmware_loaded
1131+
_device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
1132+
_device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
11251133

11261134
// Perform default startup
11271135
// Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.
@@ -1301,6 +1309,8 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
13011309
#endif
13021310

13031311
_device._firmware_loaded = false; // Initialize _firmware_loaded
1312+
_device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
1313+
_device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
13041314

13051315
// Perform default startup
13061316
// Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.

0 commit comments

Comments
 (0)