Skip to content

Commit d5e82cb

Browse files
PaulZCPaulZC
committed
More DMP functionality
1 parent f63c33b commit d5e82cb

File tree

7 files changed

+347
-151
lines changed

7 files changed

+347
-151
lines changed

‎examples/Arduino/Example5_DMP/Example5_DMP.ino‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -167,6 +167,11 @@ void setup() {
167167
// X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02
168168
// Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12
169169
// Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
170+
// Magnetometer full scale is +/- 4900uT so _I think_ we need to multiply by 2^30 / 4900 = 0x000357FA
171+
// The magnetometer Y and Z axes are reversed compared to the accelerometer so we'll invert those
172+
// const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
173+
// const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
174+
// const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
170175
const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
171176
const unsigned char mountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00};
172177
const unsigned char mountMultiplierMinus[4] = {0xC0, 0x00, 0x00, 0x00};
@@ -195,6 +200,12 @@ void setup() {
195200
// Enable DMP interrupt
196201
//success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
197202

203+
// Enable the DMP orientation sensor
204+
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
205+
206+
// Set the DMP orientation sensor rate to 25Hz
207+
success &= (myICM.setDMPODRrate(INV_ICM20948_SENSOR_ORIENTATION, 25) == ICM_20948_Stat_Ok);
208+
198209
// Check success
199210
if( success )
200211
SERIAL_PORT.println("DMP enabled!");

‎src/ICM_20948.cpp‎

Lines changed: 24 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -985,11 +985,21 @@ ICM_20948_Status_e ICM_20948::setDMPstartAddress(unsigned short address)
985985
return ICM_20948_Stat_DMPNotSupported;
986986
}
987987

988-
ICM_20948_Status_e ICM_20948::enableSensor(enum inv_icm20948_sensor sensor, bool enable)
988+
ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable)
989989
{
990-
if (_device._dmp_firmware_available == true) // Should we attempt to set the start address?
990+
if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor?
991991
{
992-
status = inv_icm20948_enable_sensor(&_device, sensor, enable == true ? 1 : 0);
992+
status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0);
993+
return status;
994+
}
995+
return ICM_20948_Stat_DMPNotSupported;
996+
}
997+
998+
ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable)
999+
{
1000+
if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor interrupt?
1001+
{
1002+
status = inv_icm20948_enable_dmp_sensor_int(&_device, sensor, enable == true ? 1 : 0);
9931003
return status;
9941004
}
9951005
return ICM_20948_Stat_DMPNotSupported;
@@ -1015,6 +1025,17 @@ ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int lengt
10151025
return ICM_20948_Stat_DMPNotSupported;
10161026
}
10171027

1028+
ICM_20948_Status_e ICM_20948::setDMPODRrate(enum inv_icm20948_sensor sensor, int rate)
1029+
{
1030+
if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR?
1031+
{
1032+
uint16_t period = (225 / rate) - 1;
1033+
status = inv_icm20948_set_dmp_sensor_period(&_device, sensor, period);
1034+
return status;
1035+
}
1036+
return ICM_20948_Stat_DMPNotSupported;
1037+
}
1038+
10181039
// I2C
10191040
ICM_20948_I2C::ICM_20948_I2C()
10201041
{

‎src/ICM_20948.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -175,9 +175,11 @@ class ICM_20948
175175
ICM_20948_Status_e resetDMP(void);
176176
ICM_20948_Status_e loadDMPFirmware(void);
177177
ICM_20948_Status_e setDMPstartAddress(unsigned short address = DMP_START_ADDRESS);
178-
ICM_20948_Status_e enableSensor(enum inv_icm20948_sensor sensor, bool enable = true);
178+
ICM_20948_Status_e enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable = true);
179+
ICM_20948_Status_e enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable = true);
179180
ICM_20948_Status_e writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data);
180181
ICM_20948_Status_e readDMPmems(unsigned short reg, unsigned int length, unsigned char *data);
182+
ICM_20948_Status_e setDMPODRrate(enum inv_icm20948_sensor sensor, int rate);
181183
};
182184

183185
// I2C

‎src/util/ICM_20948_C.c‎

Lines changed: 137 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -1347,16 +1347,103 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
13471347
return result;
13481348
}
13491349

1350-
ICM_20948_Status_e inv_icm20948_set_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint32_t period)
1350+
ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint16_t period)
13511351
{
1352-
// TO DO: implement this!
1352+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
1353+
1354+
if (pdev->_dmp_firmware_available == false)
1355+
return ICM_20948_Stat_DMPNotSupported;
1356+
1357+
uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
1358+
1359+
uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
1360+
1361+
if (delta == 0xFFFF)
1362+
return ICM_20948_Stat_SensorNotSupported;
1363+
1364+
unsigned char odr_reg_val[2];
1365+
odr_reg_val[0] = (unsigned char)(period >> 8);
1366+
odr_reg_val[1] = (unsigned char)(period & 0xff);
13531367

1354-
//uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
1368+
unsigned char odr_count_zero[2] = {0x00, 0x00};
1369+
1370+
result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
1371+
if (result != ICM_20948_Stat_Ok)
1372+
{
1373+
return result;
1374+
}
13551375

1356-
return ICM_20948_Stat_Ok;
1376+
result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
1377+
if (result != ICM_20948_Stat_Ok)
1378+
{
1379+
return result;
1380+
}
1381+
1382+
// Set the ODR registers and clear the ODR counters
1383+
if (delta & DMP_Data_Output_Control_1_Compass_Calibr > 0)
1384+
{
1385+
result |= inv_icm20948_write_mems(pdev, ODR_CPASS_CALIBR, 2, (const unsigned char *)&odr_reg_val);
1386+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS_CALIBR, 2, (const unsigned char *)&odr_count_zero);
1387+
}
1388+
if (delta & DMP_Data_Output_Control_1_Gyro_Calibr > 0)
1389+
{
1390+
result |= inv_icm20948_write_mems(pdev, ODR_GYRO_CALIBR, 2, (const unsigned char *)&odr_reg_val);
1391+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO_CALIBR, 2, (const unsigned char *)&odr_count_zero);
1392+
}
1393+
if (delta & DMP_Data_Output_Control_1_Pressure > 0)
1394+
{
1395+
result |= inv_icm20948_write_mems(pdev, ODR_PRESSURE, 2, (const unsigned char *)&odr_reg_val);
1396+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_PRESSURE, 2, (const unsigned char *)&odr_count_zero);
1397+
}
1398+
if (delta & DMP_Data_Output_Control_1_Geomag > 0)
1399+
{
1400+
result |= inv_icm20948_write_mems(pdev, ODR_GEOMAG, 2, (const unsigned char *)&odr_reg_val);
1401+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_GEOMAG, 2, (const unsigned char *)&odr_count_zero);
1402+
}
1403+
if (delta & DMP_Data_Output_Control_1_PQuat6 > 0)
1404+
{
1405+
result |= inv_icm20948_write_mems(pdev, ODR_PQUAT6, 2, (const unsigned char *)&odr_reg_val);
1406+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_PQUAT6, 2, (const unsigned char *)&odr_count_zero);
1407+
}
1408+
if (delta & DMP_Data_Output_Control_1_Quat9 > 0)
1409+
{
1410+
result |= inv_icm20948_write_mems(pdev, ODR_QUAT9, 2, (const unsigned char *)&odr_reg_val);
1411+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT9, 2, (const unsigned char *)&odr_count_zero);
1412+
}
1413+
if (delta & DMP_Data_Output_Control_1_Quat6 > 0)
1414+
{
1415+
result |= inv_icm20948_write_mems(pdev, ODR_QUAT6, 2, (const unsigned char *)&odr_reg_val);
1416+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_QUAT6, 2, (const unsigned char *)&odr_count_zero);
1417+
}
1418+
if (delta & DMP_Data_Output_Control_1_ALS > 0)
1419+
{
1420+
result |= inv_icm20948_write_mems(pdev, ODR_ALS, 2, (const unsigned char *)&odr_reg_val);
1421+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_ALS, 2, (const unsigned char *)&odr_count_zero);
1422+
}
1423+
if (delta & DMP_Data_Output_Control_1_Compass > 0)
1424+
{
1425+
result |= inv_icm20948_write_mems(pdev, ODR_CPASS, 2, (const unsigned char *)&odr_reg_val);
1426+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_CPASS, 2, (const unsigned char *)&odr_count_zero);
1427+
}
1428+
if (delta & DMP_Data_Output_Control_1_Gyro > 0)
1429+
{
1430+
result |= inv_icm20948_write_mems(pdev, ODR_GYRO, 2, (const unsigned char *)&odr_reg_val);
1431+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_GYRO, 2, (const unsigned char *)&odr_count_zero);
1432+
}
1433+
if (delta & DMP_Data_Output_Control_1_Accel > 0)
1434+
{
1435+
result |= inv_icm20948_write_mems(pdev, ODR_ACCEL, 2, (const unsigned char *)&odr_reg_val);
1436+
result |= inv_icm20948_write_mems(pdev, ODR_CNTR_ACCEL, 2, (const unsigned char *)&odr_count_zero);
1437+
}
1438+
1439+
// result = ICM_20948_low_power(pdev, true); // Put chip into low power state
1440+
// if (result != ICM_20948_Stat_Ok)
1441+
// return result;
1442+
1443+
return result;
13571444
}
13581445

1359-
ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state)
1446+
ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state)
13601447
{
13611448
// TO DO: figure out how to disable the sensor if state is 0
13621449

@@ -1367,59 +1454,9 @@ ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv
13671454

13681455
uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
13691456

1370-
const short inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX]=
1371-
{
1372-
// Unsupported Sensors are -1
1373-
-1, // Meta Data
1374-
-32760, //0x8008, // Accelerometer
1375-
0x0028, // Magnetic Field
1376-
0x0408, // Orientation
1377-
0x4048, // Gyroscope
1378-
0x1008, // Light
1379-
0x0088, // Pressure
1380-
-1, // Temperature
1381-
-1, // Proximity <----------- fixme
1382-
0x0808, // Gravity
1383-
-30712, // 0x8808, // Linear Acceleration
1384-
0x0408, // Rotation Vector
1385-
-1, // Humidity
1386-
-1, // Ambient Temperature
1387-
0x2008, // Magnetic Field Uncalibrated
1388-
0x0808, // Game Rotation Vector
1389-
0x4008, // Gyroscope Uncalibrated
1390-
0, // Significant Motion
1391-
0x0018, // Step Detector
1392-
0x0010, // Step Counter <----------- fixme
1393-
0x0108, // Geomagnetic Rotation Vector
1394-
-1, //ANDROID_SENSOR_HEART_RATE,
1395-
-1, //ANDROID_SENSOR_PROXIMITY,
1396-
1397-
-32760, // ANDROID_SENSOR_WAKEUP_ACCELEROMETER,
1398-
0x0028, // ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD,
1399-
0x0408, // ANDROID_SENSOR_WAKEUP_ORIENTATION,
1400-
0x4048, // ANDROID_SENSOR_WAKEUP_GYROSCOPE,
1401-
0x1008, // ANDROID_SENSOR_WAKEUP_LIGHT,
1402-
0x0088, // ANDROID_SENSOR_WAKEUP_PRESSURE,
1403-
0x0808, // ANDROID_SENSOR_WAKEUP_GRAVITY,
1404-
-30712, // ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION,
1405-
0x0408, // ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR,
1406-
-1, // ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY,
1407-
-1, // ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE,
1408-
0x2008, // ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED,
1409-
0x0808, // ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR,
1410-
0x4008, // ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED,
1411-
0x0018, // ANDROID_SENSOR_WAKEUP_STEP_DETECTOR,
1412-
0x0010, // ANDROID_SENSOR_WAKEUP_STEP_COUNTER,
1413-
0x0108, // ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR
1414-
-1, // ANDROID_SENSOR_WAKEUP_HEART_RATE,
1415-
0, // ANDROID_SENSOR_WAKEUP_TILT_DETECTOR,
1416-
0x8008, // Raw Acc
1417-
0x4048, // Raw Gyr
1418-
};
1419-
1420-
short delta = inv_androidSensor_to_control_bits[androidSensor];
1421-
1422-
if (delta == -1)
1457+
uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
1458+
1459+
if (delta == 0xFFFF)
14231460
return ICM_20948_Stat_SensorNotSupported;
14241461

14251462
unsigned char data_output_control_reg1[2];
@@ -1442,9 +1479,48 @@ ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv
14421479
// Write the sensor control bits into memory address DATA_OUT_CTL1
14431480
result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL1, 2, (const unsigned char *)&data_output_control_reg1);
14441481

1445-
// TO DO: figure out if we need to set the ODR
1446-
// TO DO: figure out if we need to update REG_PWR_MGMT
1447-
// TO DO: figure out if we need to set DATA_RDY_STATUS
1482+
// result = ICM_20948_low_power(pdev, true); // Put chip into low power state
1483+
// if (result != ICM_20948_Stat_Ok)
1484+
// return result;
1485+
1486+
return result;
1487+
}
1488+
1489+
ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state)
1490+
{
1491+
// TO DO: figure out how to disable the sensor if state is 0
1492+
1493+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
1494+
1495+
if (pdev->_dmp_firmware_available == false)
1496+
return ICM_20948_Stat_DMPNotSupported;
1497+
1498+
uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
1499+
1500+
uint16_t delta = inv_androidSensor_to_control_bits[androidSensor];
1501+
1502+
if (delta == 0xFFFF)
1503+
return ICM_20948_Stat_SensorNotSupported;
1504+
1505+
unsigned char data_output_control_reg1[2];
1506+
1507+
data_output_control_reg1[0] = (unsigned char)(delta >> 8);
1508+
data_output_control_reg1[1] = (unsigned char)(delta & 0xff);
1509+
1510+
result = ICM_20948_sleep(pdev, false); // Make sure chip is awake
1511+
if (result != ICM_20948_Stat_Ok)
1512+
{
1513+
return result;
1514+
}
1515+
1516+
result = ICM_20948_low_power(pdev, false); // Make sure chip is not in low power state
1517+
if (result != ICM_20948_Stat_Ok)
1518+
{
1519+
return result;
1520+
}
1521+
1522+
// Write the sensor control bits into memory address DATA_OUT_CTL1
1523+
result = inv_icm20948_write_mems(pdev, DATA_INTR_CTL, 2, (const unsigned char *)&data_output_control_reg1);
14481524

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

‎src/util/ICM_20948_C.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -279,8 +279,9 @@ callbacks for the user to respond to interrupt events
279279
*/
280280
ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, unsigned char *data);
281281

282-
ICM_20948_Status_e inv_icm20948_set_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint32_t period);
283-
ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state);
282+
ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint16_t period);
283+
ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state);
284+
ICM_20948_Status_e inv_icm20948_enable_dmp_sensor_int(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state);
284285
static uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor);
285286
enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor);
286287

0 commit comments

Comments
 (0)