@@ -209,19 +209,17 @@ void setup() {
209
209
// Configure the B2S Mounting Matrix
210
210
const unsigned char b2sMountMultiplierZero[4 ] = {0x00 , 0x00 , 0x00 , 0x00 };
211
211
const unsigned char b2sMountMultiplierPlus[4 ] = {0x40 , 0x00 , 0x00 , 0x00 }; // Value taken from InvenSense Nucleo example
212
- success &= (myICM.writeDMPmems (B2S_MTX_00, 4 , &mountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
213
- success &= (myICM.writeDMPmems (B2S_MTX_01, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
214
- success &= (myICM.writeDMPmems (B2S_MTX_02, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
215
- success &= (myICM.writeDMPmems (B2S_MTX_10, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
216
- success &= (myICM.writeDMPmems (B2S_MTX_11, 4 , &mountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
217
- success &= (myICM.writeDMPmems (B2S_MTX_12, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
218
- success &= (myICM.writeDMPmems (B2S_MTX_20, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
219
- success &= (myICM.writeDMPmems (B2S_MTX_21, 4 , &mountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
220
- success &= (myICM.writeDMPmems (B2S_MTX_22, 4 , &mountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
212
+ success &= (myICM.writeDMPmems (B2S_MTX_00, 4 , &b2sMountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
213
+ success &= (myICM.writeDMPmems (B2S_MTX_01, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
214
+ success &= (myICM.writeDMPmems (B2S_MTX_02, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
215
+ success &= (myICM.writeDMPmems (B2S_MTX_10, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
216
+ success &= (myICM.writeDMPmems (B2S_MTX_11, 4 , &b2sMountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
217
+ success &= (myICM.writeDMPmems (B2S_MTX_12, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
218
+ success &= (myICM.writeDMPmems (B2S_MTX_20, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
219
+ success &= (myICM.writeDMPmems (B2S_MTX_21, 4 , &b2sMountMultiplierZero [0 ]) == ICM_20948_Stat_Ok);
220
+ success &= (myICM.writeDMPmems (B2S_MTX_22, 4 , &b2sMountMultiplierPlus [0 ]) == ICM_20948_Stat_Ok);
221
221
222
222
// Configure the DMP Gyro Scaling Factor
223
- // const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example
224
- // success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok);
225
223
// @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where
226
224
// 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ...
227
225
// 10=102.2727Hz sample rate, ... etc.
@@ -249,7 +247,7 @@ void setup() {
249
247
// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
250
248
const unsigned char accelAVar[4 ] = {0x0B , 0x6D , 0xB6 , 0xDB }; // 56Hz
251
249
// const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
252
- success &= (myICM.writeDMPmems (ACCEL_A_VAR, 4 , &accelAlphaVar [0 ]) == ICM_20948_Stat_Ok);
250
+ success &= (myICM.writeDMPmems (ACCEL_A_VAR, 4 , &accelAVar [0 ]) == ICM_20948_Stat_Ok);
253
251
254
252
// Configure the Accel Cal Rate
255
253
const unsigned char accelCalRate[4 ] = {0x00 , 0x00 }; // Value taken from InvenSense Nucleo example
@@ -281,7 +279,7 @@ void setup() {
281
279
// INV_ICM20948_SENSOR_LINEAR_ACCELERATION (16-bit accel + 32-bit 6-axis quaternion)
282
280
// INV_ICM20948_SENSOR_ORIENTATION (32-bit 9-axis quaternion + heading accuracy)
283
281
284
- // Enable the DMP Game Rotation Vector sensor
282
+ // Enable the DMP Game Rotation Vector sensor (Quat6)
285
283
success &= (myICM.enableDMPSensor (INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);
286
284
287
285
// Enable additional sensors / features
@@ -322,6 +320,8 @@ void setup() {
322
320
{
323
321
SERIAL_PORT.println (F (" Enable DMP failed!" ));
324
322
SERIAL_PORT.println (F (" Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..." ));
323
+ while (1 )
324
+ ; // Do nothing more
325
325
}
326
326
}
327
327
@@ -345,18 +345,18 @@ void loop()
345
345
// if ( data.header < 0x10) SERIAL_PORT.print( "0" );
346
346
// SERIAL_PORT.println( data.header, HEX );
347
347
348
- if ( (data.header & DMP_header_bitmap_Quat9 ) > 0 ) // Check for orientation data (Quat9)
348
+ if ( (data.header & DMP_header_bitmap_Quat6 ) > 0 ) // Check for orientation data (Quat9)
349
349
{
350
350
// Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1.
351
351
// In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.
352
352
// The quaternion data is scaled by 2^30.
353
353
354
- // 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);
354
+ // SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat6 .Data.Q1, data.Quat6 .Data.Q2, data.Quat9.Data.Q3, data.Quat6 .Data.Accuracy);
355
355
356
356
// Scale to +/- 1
357
- double q1 = ((double )data.Quat9 .Data .Q1 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
358
- double q2 = ((double )data.Quat9 .Data .Q2 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
359
- double q3 = ((double )data.Quat9 .Data .Q3 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
357
+ double q1 = ((double )data.Quat6 .Data .Q1 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
358
+ double q2 = ((double )data.Quat6 .Data .Q2 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
359
+ double q3 = ((double )data.Quat6 .Data .Q3 ) / 1073741824.0 ; // Convert to double. Divide by 2^30
360
360
361
361
SERIAL_PORT.print (F (" Q1:" ));
362
362
SERIAL_PORT.print (q1, 3 );
0 commit comments