Skip to content

Commit f6ae0a1

Browse files
PaulZCPaulZC
committed
Correcting inv_icm20948_enable_dmp_sensor. It now sets DATA_RDY_STATUS and MOTION_EVENT_CTL.
1 parent ef64ab0 commit f6ae0a1

File tree

6 files changed

+151
-16
lines changed

6 files changed

+151
-16
lines changed

‎DMP.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -516,7 +516,7 @@ Brace yourself. Here it is:
516516
* Set register 0x7C (Memory Start Address) to 0xB0
517517
* Write 0x06666666 to memory (91 * 16 + 0 == Accel Alpha Var)
518518
* Set register 0x7C (Memory Start Address) to 0xC0
519-
* Write 0x39999A to memory (92 * 16 + 0 == Accel A Var)
519+
* Write 0x3999999A to memory (92 * 16 + 0 == Accel A Var)
520520
* Set register 0x7C (Memory Start Address) to 0xE4
521521
* Write 0x0000 to memory (94 * 16 + 4 == Accel Cal Rate)
522522
* .....

‎examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -212,6 +212,18 @@ void setup() {
212212
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
213213
success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
214214

215+
// Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
216+
const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
217+
success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
218+
219+
// Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
220+
const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
221+
success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
222+
223+
// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
224+
const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
225+
success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
226+
215227
// Enable DMP interrupt
216228
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
217229
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
@@ -300,7 +312,14 @@ void loop()
300312
float q2 = ((float)data.Quat9.Data.Q2) / 1073741824.0; // Convert to float. Divide by 2^30
301313
float q3 = ((float)data.Quat9.Data.Q3) / 1073741824.0; // Convert to float. Divide by 2^30
302314

303-
SERIAL_PORT.printf("Q1:%.3f Q2:%.3f Q3:%.3f Accuracy:%d\r\n", q1, q2, q3, data.Quat9.Data.Accuracy);
315+
SERIAL_PORT.print(F("Q1:"));
316+
SERIAL_PORT.print(q1, 3);
317+
SERIAL_PORT.print(F(" Q2:"));
318+
SERIAL_PORT.print(q2, 3);
319+
SERIAL_PORT.print(F(" Q3:"));
320+
SERIAL_PORT.print(q3, 3);
321+
SERIAL_PORT.print(F(" Accuracy:"));
322+
SERIAL_PORT.println(data.Quat9.Data.Accuracy);
304323
}
305324
}
306325

‎examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino

Lines changed: 18 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -212,6 +212,18 @@ void setup() {
212212
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
213213
success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
214214

215+
// Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
216+
const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
217+
success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
218+
219+
// Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
220+
const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
221+
success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
222+
223+
// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
224+
const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
225+
success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
226+
215227
// Enable DMP interrupt
216228
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
217229
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
@@ -241,11 +253,6 @@ void setup() {
241253
// Set the DMP Output Data Rate for Quat6 to 12Hz.
242254
//success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 12) == ICM_20948_Stat_Ok);
243255

244-
// Set the DMP Data Ready Status register
245-
const uint16_t dsrBits = DMP_Data_ready_Gyro | DMP_Data_ready_Accel;
246-
const unsigned char drsReg[2] = {(const unsigned char)(dsrBits >> 8), (const unsigned char)(dsrBits & 0xFF)};
247-
success &= (myICM.writeDMPmems(DATA_RDY_STATUS, 2, &drsReg[0]) == ICM_20948_Stat_Ok);
248-
249256
// Enable the FIFO
250257
success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
251258

@@ -323,7 +330,12 @@ void loop()
323330
double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
324331
double yaw = atan2(t3, t4) * 180.0 / PI;
325332

326-
SERIAL_PORT.printf("Roll:%.1f Pitch:%.1f Yaw:%.1f\r\n", roll, pitch, yaw);
333+
SERIAL_PORT.print(F("Roll:"));
334+
SERIAL_PORT.print(roll, 1);
335+
SERIAL_PORT.print(F(" Pitch:"));
336+
SERIAL_PORT.print(pitch, 1);
337+
SERIAL_PORT.print(F(" Yaw:"));
338+
SERIAL_PORT.println(yaw, 1);
327339
}
328340
}
329341

‎examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino

Lines changed: 18 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -212,6 +212,18 @@ void setup() {
212212
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
213213
success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
214214

215+
// Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
216+
const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
217+
success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
218+
219+
// Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
220+
const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
221+
success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
222+
223+
// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
224+
const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
225+
success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
226+
215227
// Enable DMP interrupt
216228
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
217229
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
@@ -241,11 +253,6 @@ void setup() {
241253
// Set the DMP Output Data Rates to 2Hz
242254
//success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 2) == ICM_20948_Stat_Ok); // ** Note: comment this line to leave the data rate at the maximum **
243255

244-
// Set the DMP Data Ready Status register
245-
const uint16_t dsrBits = DMP_Data_ready_Gyro | DMP_Data_ready_Accel; // Value taken from InvenSense Nucleo example
246-
const unsigned char drsReg[2] = {(const unsigned char)(dsrBits >> 8), (const unsigned char)(dsrBits & 0xFF)};
247-
success &= (myICM.writeDMPmems(DATA_RDY_STATUS, 2, &drsReg[0]) == ICM_20948_Stat_Ok);
248-
249256
// Enable the FIFO
250257
success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
251258

@@ -293,7 +300,12 @@ void loop()
293300
float acc_y = (float)data.Raw_Accel.Data.Y;
294301
float acc_z = (float)data.Raw_Accel.Data.Z;
295302

296-
SERIAL_PORT.printf("X:%f Y:%f Z:%f\r\n", acc_x, acc_y, acc_z);
303+
SERIAL_PORT.print(F("X:"));
304+
SERIAL_PORT.print(acc_x);
305+
SERIAL_PORT.print(F(" Y:"));
306+
SERIAL_PORT.print(acc_y);
307+
SERIAL_PORT.print(F(" Z:"));
308+
SERIAL_PORT.println(acc_z);
297309
}
298310
}
299311

‎src/util/ICM_20948_C.c

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1484,6 +1484,9 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
14841484

14851485
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
14861486

1487+
uint16_t inv_event_control = 0;
1488+
uint16_t data_rdy_status = 0;
1489+
14871490
if (pdev->_dmp_firmware_available == false)
14881491
return ICM_20948_Stat_DMPNotSupported;
14891492

@@ -1540,6 +1543,81 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
15401543
data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
15411544
data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
15421545
result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL2, 2, (const unsigned char *)&data_output_control_reg);
1546+
if (result != ICM_20948_Stat_Ok)
1547+
{
1548+
return result;
1549+
}
1550+
1551+
// Check which bits need to be set in the Data Ready Status and Motion Event Control registers
1552+
// Convert androidSensor into a bit mask and compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
1553+
unsigned long androidSensorAsBitMask;
1554+
if (androidSensor < 32)
1555+
{
1556+
androidSensorAsBitMask = 1L << androidSensor;
1557+
if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0)
1558+
{
1559+
data_rdy_status |= DMP_Data_ready_Accel;
1560+
inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
1561+
}
1562+
if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0)
1563+
{
1564+
data_rdy_status |= DMP_Data_ready_Gyro;
1565+
inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
1566+
}
1567+
if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0)
1568+
{
1569+
data_rdy_status |= DMP_Data_ready_Secondary_Compass;
1570+
inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
1571+
}
1572+
}
1573+
else
1574+
{
1575+
androidSensorAsBitMask = 1L << (androidSensor - 32);
1576+
if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0)
1577+
{
1578+
data_rdy_status |= DMP_Data_ready_Accel;
1579+
inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
1580+
}
1581+
if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0)
1582+
{
1583+
data_rdy_status |= DMP_Data_ready_Gyro;
1584+
inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
1585+
}
1586+
if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0)
1587+
{
1588+
data_rdy_status |= DMP_Data_ready_Secondary_Compass;
1589+
inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
1590+
}
1591+
}
1592+
data_output_control_reg[0] = (unsigned char)(data_rdy_status >> 8);
1593+
data_output_control_reg[1] = (unsigned char)(data_rdy_status & 0xff);
1594+
result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
1595+
if (result != ICM_20948_Stat_Ok)
1596+
{
1597+
return result;
1598+
}
1599+
1600+
// Check which extra bits need to be set in the Motion Event Control register
1601+
if ((delta & DMP_Data_Output_Control_1_Quat9) > 0)
1602+
{
1603+
inv_event_control |= DMP_Motion_Event_Control_9axis;
1604+
}
1605+
if (((delta & DMP_Data_Output_Control_1_Step_Detector) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_0) > 0)
1606+
|| ((delta & DMP_Data_Output_Control_1_Step_Ind_1) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_2) > 0))
1607+
{
1608+
inv_event_control |= DMP_Motion_Event_Control_Pedometer_Interrupt;
1609+
}
1610+
if ((delta & DMP_Data_Output_Control_1_Geomag) > 0)
1611+
{
1612+
inv_event_control |= DMP_Motion_Event_Control_Geomag;
1613+
}
1614+
data_output_control_reg[0] = (unsigned char)(inv_event_control >> 8);
1615+
data_output_control_reg[1] = (unsigned char)(inv_event_control & 0xff);
1616+
result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
1617+
if (result != ICM_20948_Stat_Ok)
1618+
{
1619+
return result;
1620+
}
15431621

15441622
// result = ICM_20948_low_power(pdev, true); // Put chip into low power state
15451623
// if (result != ICM_20948_Stat_Ok)

‎src/util/ICM_20948_DMP.h

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ extern "C" {
4747
// indicates to DMP which sensors are available
4848
/* 1: gyro samples available
4949
2: accel samples available
50-
8: secondary samples available */
50+
8: secondary compass samples available */
5151
#define DATA_RDY_STATUS (8 * 16 + 10) // 16-bit: indicates to DMP which sensors are available
5252

5353
// batch mode
@@ -388,6 +388,17 @@ enum ANDROID_SENSORS {
388388
GENERAL_SENSORS_MAX // 51
389389
};
390390

391+
// Determines which base sensor needs to be on based upon ANDROID_SENSORS 0-31
392+
#define INV_NEEDS_ACCEL_MASK ((1L<<1)| (1L<<3)| (1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)| (1L<<17)|(1L<<18)|(1L<<19)|(1L<<20)|(1<<23)| (1<<25)| (1<<29)|(1<<30)|(1<<31))
393+
#define INV_NEEDS_GYRO_MASK ( (1L<<3)|(1L<<4)|(1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)|(1L<<16)| (1<<25)|(1<<26)|(1<<29)|(1<<30)|(1<<31))
394+
#define INV_NEEDS_COMPASS_MASK ( (1L<<2)|(1L<<3)| (1L<<11)|(1L<<14)| (1L<<20)| (1<<24)|(1<<25)| (1<<31))
395+
#define INV_NEEDS_PRESSURE ((1L<<6)|(1<<28))
396+
397+
// Determines which base sensor needs to be on based upon ANDROID_SENSORS 32-
398+
#define INV_NEEDS_ACCEL_MASK1 ( (1<<3)| (1<<5)|(1<<6)|(1<<7)|(1<<9)|(1<<10)) // I.e. 35, 37, 38, 39, 41, 42
399+
#define INV_NEEDS_GYRO_MASK1 ( (1<<3)|(1<<4) |(1<<11)) // I.e. 35, 36, 43
400+
#define INV_NEEDS_COMPASS_MASK1 ((1<<2)| (1<<7)) // I.e. 34 and 39
401+
391402
enum DMP_Data_Ready_Status_Register_Bits
392403
{
393404
DMP_Data_ready_Gyro = 0x0001, // Gyro samples available
@@ -430,16 +441,19 @@ enum DMP_Data_Output_Control_2_Register_Bits
430441
enum DMP_Motion_Event_Control_Register_Bits
431442
{
432443
DMP_Motion_Event_Control_Activity_Recog_Pedom_Accel = 0x0002, // Activity Recognition / Pedometer accel only
444+
DMP_Motion_Event_Control_Bring_Look_To_See = 0x0004,
433445
DMP_Motion_Event_Control_Geomag = 0x0008, // Geomag rv
434446
DMP_Motion_Event_Control_Pickup = 0x0010,
447+
DMP_Motion_Event_Control_BTS = 0x0020,
435448
DMP_Motion_Event_Control_9axis = 0x0040,
436449
DMP_Motion_Event_Control_Compass_Calibr = 0x0080,
437450
DMP_Motion_Event_Control_Gyro_Calibr = 0x0100,
438451
DMP_Motion_Event_Control_Accel_Calibr = 0x0200,
439452
DMP_Motion_Event_Control_Significant_Motion_Det = 0x0800,
440453
DMP_Motion_Event_Control_Tilt_Interrupt = 0x1000,
441454
DMP_Motion_Event_Control_Pedometer_Interrupt = 0x2000,
442-
DMP_Motion_Event_Control_Activity_Recog_Pedom = 0x4000
455+
DMP_Motion_Event_Control_Activity_Recog_Pedom = 0x4000,
456+
DMP_Motion_Event_Control_BAC_Wearable = 0x8000
443457
};
444458

445459
enum DMP_Header_Bitmap

0 commit comments

Comments
 (0)