Skip to content

Commit 552b502

Browse files
PaulZCPaulZC
committed
Starting to add DMP enableSensor functionality
1 parent 7044e2e commit 552b502

File tree

6 files changed

+291
-52
lines changed

6 files changed

+291
-52
lines changed

‎examples/Arduino/Example5_DMP/Example5_DMP.ino‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,17 @@ void setup() {
6565
}
6666
}
6767

68+
SERIAL_PORT.println("Device connected!");
69+
70+
myICM.enableSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR); // Enable the Game Rotation Vector
71+
72+
if( myICM.status == ICM_20948_Stat_Ok )
73+
SERIAL_PORT.println("INV_ICM20948_SENSOR_ROTATION_VECTOR enabled!");
74+
else
75+
{
76+
SERIAL_PORT.println("INV_ICM20948_SENSOR_ROTATION_VECTOR failed! Status is: ");
77+
SERIAL_PORT.println( myICM.statusString() );
78+
}
6879
}
6980

7081
void loop() {

‎src/ICM_20948.cpp‎

Lines changed: 31 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,9 @@ void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat)
107107
case ICM_20948_Stat_SensorNotSupported:
108108
debugPrint(F("Sensor Not Supported"));
109109
break;
110+
case ICM_20948_Stat_DMPNotSupported:
111+
debugPrint(F("DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?"));
112+
break;
110113
case ICM_20948_Stat_DMPVerifyFail:
111114
debugPrint(F("DMP Firmware Verification Failed"));
112115
break;
@@ -780,14 +783,17 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
780783
return status;
781784
}
782785

783-
retval = loadDMPFirmware();
784-
if (retval != ICM_20948_Stat_Ok)
786+
if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware?
785787
{
786-
debugPrint(F("ICM_20948::startupDefault: loadDMPFirmware returned: "));
787-
debugPrintStatus(retval);
788-
debugPrintln(F(""));
789-
status = retval;
790-
return status;
788+
retval = loadDMPFirmware();
789+
if (retval != ICM_20948_Stat_Ok)
790+
{
791+
debugPrint(F("ICM_20948::startupDefault: loadDMPFirmware returned: "));
792+
debugPrintStatus(retval);
793+
debugPrintln(F(""));
794+
status = retval;
795+
return status;
796+
}
791797
}
792798

793799
retval = swReset();
@@ -921,6 +927,12 @@ ICM_20948_Status_e ICM_20948::loadDMPFirmware(void)
921927
return status;
922928
}
923929

930+
ICM_20948_Status_e ICM_20948::enableSensor(enum inv_icm20948_sensor sensor, bool enable)
931+
{
932+
status = inv_icm20948_enable_sensor(&_device, sensor, enable == true ? 1 : 0);
933+
return status;
934+
}
935+
924936
// I2C
925937
ICM_20948_I2C::ICM_20948_I2C()
926938
{
@@ -961,6 +973,12 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
961973
// Link the serif
962974
_device._serif = &_serif;
963975

976+
#if defined(ICM_20948_USE_DMP)
977+
_device._dmp_firmware_available = true; // Initialize _dmp_firmware_available
978+
#else
979+
_device._dmp_firmware_available = false; // Initialize _dmp_firmware_available
980+
#endif
981+
964982
_device._firmware_loaded = false; // Initialize _firmware_loaded
965983

966984
// Perform default startup
@@ -1133,6 +1151,12 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
11331151
// Link the serif
11341152
_device._serif = &_serif;
11351153

1154+
#if defined(ICM_20948_USE_DMP)
1155+
_device._dmp_firmware_available = true; // Initialize _dmp_firmware_available
1156+
#else
1157+
_device._dmp_firmware_available = false; // Initialize _dmp_firmware_available
1158+
#endif
1159+
11361160
_device._firmware_loaded = false; // Initialize _firmware_loaded
11371161

11381162
// Perform default startup

‎src/ICM_20948.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ A C++ interface to the ICM-20948
77
#ifndef _ICM_20948_H_
88
#define _ICM_20948_H_
99

10-
#include "util/ICM_20948_C.h" // The C backbone
10+
#include "util/ICM_20948_C.h" // The C backbone. ICM_20948_USE_DMP is defined in here.
1111
#include "util/AK09916_REGISTERS.h"
1212

1313
#include "Arduino.h" // Arduino support
@@ -165,6 +165,7 @@ class ICM_20948
165165

166166
//DMP
167167
ICM_20948_Status_e loadDMPFirmware(void);
168+
ICM_20948_Status_e enableSensor(enum inv_icm20948_sensor sensor, bool enable = true);
168169
};
169170

170171
// I2C

‎src/util/ICM_20948_C.c‎

Lines changed: 141 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -865,7 +865,11 @@ ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t *pdev, ICM_20948_AGMT_t
865865

866866
ICM_20948_Status_e ICM_20948_firmware_load(ICM_20948_Device_t *pdev)
867867
{
868-
return (inv_icm20948_firmware_load(pdev, dmp3_image, sizeof(dmp3_image), DMP_LOAD_START));
868+
#if defined(ICM_20948_USE_DMP)
869+
return (inv_icm20948_firmware_load(pdev, dmp3_image, sizeof(dmp3_image), DMP_LOAD_START));
870+
#else
871+
return ICM_20948_Stat_DMPNotSupported;
872+
#endif
869873
}
870874

871875
/** @brief Loads the DMP firmware from SRAM
@@ -1159,42 +1163,139 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
11591163
return result;
11601164
}
11611165

1162-
// _device._serif.write
1163-
//
1164-
// ICM_20948_Status_e inv_icm20948_write_reg(ICM_20948_Device_t *pdev, uint8_t reg, const uint8_t * buf, uint32_t len)
1165-
// {
1166-
// return inv_icm20948_serif_write_reg(&s->serif, reg, buf, len);
1167-
// }
1168-
//
1169-
// static inline ICM_20948_Status_e inv_icm20948_serif_write_reg(ICM_20948_Device_t *pdev,
1170-
// uint8_t reg, const uint8_t * buf, uint32_t len)
1171-
// {
1172-
// assert(s);
1173-
//
1174-
// if(len > s->max_write)
1175-
// return INV_ERROR_SIZE;
1176-
//
1177-
// if(s->write_reg(s->context, reg, buf, len) != 0)
1178-
// return INV_ERROR_TRANSPORT;
1179-
//
1180-
// return 0;
1181-
// }
1182-
//
1183-
// ICM_20948_Status_e inv_icm20948_read_reg(ICM_20948_Device_t *pdev, uint8_t reg, uint8_t * buf, uint32_t len)
1184-
// {
1185-
// return inv_icm20948_serif_read_reg(&s->serif, reg, buf, len);
1186-
// }
1187-
//
1188-
// static inline ICM_20948_Status_e inv_icm20948_serif_read_reg(ICM_20948_Device_t *pdev,
1189-
// uint8_t reg, uint8_t * buf, uint32_t len)
1190-
// {
1191-
// assert(s);
1192-
//
1193-
// if(len > s->max_read)
1194-
// return INV_ERROR_SIZE;
1195-
//
1196-
// if(s->read_reg(s->context, reg, buf, len) != 0)
1197-
// return INV_ERROR_TRANSPORT;
1198-
//
1199-
// return 0;
1200-
// }
1166+
ICM_20948_Status_e inv_icm20948_set_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint32_t period)
1167+
{
1168+
//uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
1169+
1170+
return ICM_20948_Stat_Ok;
1171+
}
1172+
1173+
ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state)
1174+
{
1175+
if (pdev->_dmp_firmware_available == false)
1176+
return ICM_20948_Stat_DMPNotSupported;
1177+
1178+
uint8_t androidSensor = sensor_type_2_android_sensor(sensor);
1179+
1180+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
1181+
// unsigned short inv_event_control = 0;
1182+
// unsigned short data_rdy_status = 0;
1183+
// unsigned long steps=0;
1184+
const short inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX]=
1185+
{
1186+
// Unsupported Sensors are -1
1187+
-1, // Meta Data
1188+
-32760, //0x8008, // Accelerometer
1189+
0x0028, // Magnetic Field
1190+
0x0408, // Orientation
1191+
0x4048, // Gyroscope
1192+
0x1008, // Light
1193+
0x0088, // Pressure
1194+
-1, // Temperature
1195+
-1, // Proximity <----------- fixme
1196+
0x0808, // Gravity
1197+
-30712, // 0x8808, // Linear Acceleration
1198+
0x0408, // Rotation Vector
1199+
-1, // Humidity
1200+
-1, // Ambient Temperature
1201+
0x2008, // Magnetic Field Uncalibrated
1202+
0x0808, // Game Rotation Vector
1203+
0x4008, // Gyroscope Uncalibrated
1204+
0, // Significant Motion
1205+
0x0018, // Step Detector
1206+
0x0010, // Step Counter <----------- fixme
1207+
0x0108, // Geomagnetic Rotation Vector
1208+
-1, //ANDROID_SENSOR_HEART_RATE,
1209+
-1, //ANDROID_SENSOR_PROXIMITY,
1210+
1211+
-32760, // ANDROID_SENSOR_WAKEUP_ACCELEROMETER,
1212+
0x0028, // ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD,
1213+
0x0408, // ANDROID_SENSOR_WAKEUP_ORIENTATION,
1214+
0x4048, // ANDROID_SENSOR_WAKEUP_GYROSCOPE,
1215+
0x1008, // ANDROID_SENSOR_WAKEUP_LIGHT,
1216+
0x0088, // ANDROID_SENSOR_WAKEUP_PRESSURE,
1217+
0x0808, // ANDROID_SENSOR_WAKEUP_GRAVITY,
1218+
-30712, // ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION,
1219+
0x0408, // ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR,
1220+
-1, // ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY,
1221+
-1, // ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE,
1222+
0x2008, // ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED,
1223+
0x0808, // ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR,
1224+
0x4008, // ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED,
1225+
0x0018, // ANDROID_SENSOR_WAKEUP_STEP_DETECTOR,
1226+
0x0010, // ANDROID_SENSOR_WAKEUP_STEP_COUNTER,
1227+
0x0108, // ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR
1228+
-1, // ANDROID_SENSOR_WAKEUP_HEART_RATE,
1229+
0, // ANDROID_SENSOR_WAKEUP_TILT_DETECTOR,
1230+
0x8008, // Raw Acc
1231+
0x4048, // Raw Gyr
1232+
};
1233+
1234+
short delta = inv_androidSensor_to_control_bits[androidSensor];
1235+
1236+
if (delta == -1)
1237+
return ICM_20948_Stat_SensorNotSupported;
1238+
1239+
unsigned char data_output_control_reg1[2];
1240+
1241+
data_output_control_reg1[0] = (unsigned char)(delta >> 8);
1242+
data_output_control_reg1[1] = (unsigned char)(delta & 0xff);
1243+
1244+
result = ICM_20948_execute_w(pdev, DATA_OUT_CTL1, data_output_control_reg1, 2);
1245+
1246+
return result;
1247+
}
1248+
1249+
static uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor)
1250+
{
1251+
switch(sensor) {
1252+
case INV_ICM20948_SENSOR_ACCELEROMETER: return ANDROID_SENSOR_ACCELEROMETER;
1253+
case INV_ICM20948_SENSOR_GYROSCOPE: return ANDROID_SENSOR_GYROSCOPE;
1254+
case INV_ICM20948_SENSOR_RAW_ACCELEROMETER: return ANDROID_SENSOR_RAW_ACCELEROMETER;
1255+
case INV_ICM20948_SENSOR_RAW_GYROSCOPE: return ANDROID_SENSOR_RAW_GYROSCOPE;
1256+
case INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED: return ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED;
1257+
case INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED: return ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED;
1258+
case INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON: return ANDROID_SENSOR_ACTIVITY_CLASSIFICATON;
1259+
case INV_ICM20948_SENSOR_STEP_DETECTOR: return ANDROID_SENSOR_STEP_DETECTOR;
1260+
case INV_ICM20948_SENSOR_STEP_COUNTER: return ANDROID_SENSOR_STEP_COUNTER;
1261+
case INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR: return ANDROID_SENSOR_GAME_ROTATION_VECTOR;
1262+
case INV_ICM20948_SENSOR_ROTATION_VECTOR: return ANDROID_SENSOR_ROTATION_VECTOR;
1263+
case INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR: return ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR;
1264+
case INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD: return ANDROID_SENSOR_GEOMAGNETIC_FIELD;
1265+
case INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION: return ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION;
1266+
case INV_ICM20948_SENSOR_FLIP_PICKUP: return ANDROID_SENSOR_FLIP_PICKUP;
1267+
case INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR: return ANDROID_SENSOR_WAKEUP_TILT_DETECTOR;
1268+
case INV_ICM20948_SENSOR_GRAVITY: return ANDROID_SENSOR_GRAVITY;
1269+
case INV_ICM20948_SENSOR_LINEAR_ACCELERATION: return ANDROID_SENSOR_LINEAR_ACCELERATION;
1270+
case INV_ICM20948_SENSOR_ORIENTATION: return ANDROID_SENSOR_ORIENTATION;
1271+
case INV_ICM20948_SENSOR_B2S: return ANDROID_SENSOR_B2S;
1272+
default: return ANDROID_SENSOR_NUM_MAX;
1273+
}
1274+
}
1275+
1276+
enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor)
1277+
{
1278+
switch(sensor) {
1279+
case ANDROID_SENSOR_ACCELEROMETER: return INV_ICM20948_SENSOR_ACCELEROMETER;
1280+
case ANDROID_SENSOR_GYROSCOPE: return INV_ICM20948_SENSOR_GYROSCOPE;
1281+
case ANDROID_SENSOR_RAW_ACCELEROMETER: return INV_ICM20948_SENSOR_RAW_ACCELEROMETER;
1282+
case ANDROID_SENSOR_RAW_GYROSCOPE: return INV_ICM20948_SENSOR_RAW_GYROSCOPE;
1283+
case ANDROID_SENSOR_MAGNETIC_FIELD_UNCALIBRATED: return INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED;
1284+
case ANDROID_SENSOR_GYROSCOPE_UNCALIBRATED: return INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED;
1285+
case ANDROID_SENSOR_ACTIVITY_CLASSIFICATON: return INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON;
1286+
case ANDROID_SENSOR_STEP_DETECTOR: return INV_ICM20948_SENSOR_STEP_DETECTOR;
1287+
case ANDROID_SENSOR_STEP_COUNTER: return INV_ICM20948_SENSOR_STEP_COUNTER;
1288+
case ANDROID_SENSOR_GAME_ROTATION_VECTOR: return INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR;
1289+
case ANDROID_SENSOR_ROTATION_VECTOR: return INV_ICM20948_SENSOR_ROTATION_VECTOR;
1290+
case ANDROID_SENSOR_GEOMAGNETIC_ROTATION_VECTOR: return INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR;
1291+
case ANDROID_SENSOR_GEOMAGNETIC_FIELD: return INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD;
1292+
case ANDROID_SENSOR_WAKEUP_SIGNIFICANT_MOTION: return INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION;
1293+
case ANDROID_SENSOR_FLIP_PICKUP: return INV_ICM20948_SENSOR_FLIP_PICKUP;
1294+
case ANDROID_SENSOR_WAKEUP_TILT_DETECTOR: return INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR;
1295+
case ANDROID_SENSOR_GRAVITY: return INV_ICM20948_SENSOR_GRAVITY;
1296+
case ANDROID_SENSOR_LINEAR_ACCELERATION: return INV_ICM20948_SENSOR_LINEAR_ACCELERATION;
1297+
case ANDROID_SENSOR_ORIENTATION: return INV_ICM20948_SENSOR_ORIENTATION;
1298+
case ANDROID_SENSOR_B2S: return INV_ICM20948_SENSOR_B2S;
1299+
default: return INV_ICM20948_SENSOR_MAX;
1300+
}
1301+
}

‎src/util/ICM_20948_C.h‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ The imementation of the interface is flexible
1515
#include "ICM_20948_REGISTERS.h"
1616
#include "ICM_20948_ENUMERATIONS.h" // This is to give users access to usable value definiitons
1717
#include "AK09916_ENUMERATIONS.h"
18+
#include "ICM_20948_SensorTypes.h"
1819

1920
#ifdef __cplusplus
2021
extern "C"
@@ -23,6 +24,10 @@ extern "C"
2324

2425
extern int memcmp(const void *, const void *, size_t); // Avoid compiler warnings
2526

27+
// Define if the DMP will be supported
28+
// Note: you must have 93KBytes of memory available to store the DMP firmware!
29+
#define ICM_20948_USE_DMP // Uncomment this line to enable DMP support.
30+
2631
#define ICM_20948_I2C_ADDR_AD0 0x68 // Or 0x69 when AD0 is high
2732
#define ICM_20948_I2C_ADDR_AD1 0x69 //
2833
#define ICM_20948_WHOAMI 0xEA
@@ -48,6 +53,7 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
4853
ICM_20948_Stat_InvalSensor, // Tried to apply a function to a sensor that does not support it (e.g. DLPF to the temperature sensor)
4954
ICM_20948_Stat_NoData,
5055
ICM_20948_Stat_SensorNotSupported,
56+
ICM_20948_Stat_DMPNotSupported, // DMP not supported (no #define ICM_20948_USE_DMP)
5157
ICM_20948_Stat_DMPVerifyFail, // DMP was written but did not verify correctly
5258

5359
ICM_20948_Stat_NUM,
@@ -148,16 +154,19 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
148154
typedef struct
149155
{
150156
const ICM_20948_Serif_t *_serif; // Pointer to the assigned Serif (Serial Interface) vtable
157+
bool _dmp_firmware_available; // Indicates if the DMP firmware has been included. It
151158
bool _firmware_loaded; // Indicates if DMP has been loaded
152159
} ICM_20948_Device_t; // Definition of device struct type
153160

154161
/*
155162
* Icm20948 device require a DMP image to be loaded on init
156163
* Provide such images by mean of a byte array
157164
*/
165+
#if defined(ICM_20948_USE_DMP) // Only include the 93KBytes of DMP if ICM_20948_USE_DMP is defined
158166
const uint8_t dmp3_image[] = {
159167
#include "icm20948_img.dmp3a.h"
160168
};
169+
#endif
161170

162171
// Here's the list of what I want to be able to do:
163172
/*
@@ -260,10 +269,10 @@ callbacks for the user to respond to interrupt events
260269
*/
261270
ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, unsigned char *data);
262271

263-
// ICM_20948_Status_e inv_icm20948_write_reg(ICM_20948_Device_t *pdev, uint8_t reg, const uint8_t * buf, uint32_t len);
264-
// ICM_20948_Status_e inv_icm20948_serif_write_reg(ICM_20948_Device_t *pdev, uint8_t reg, const uint8_t * buf, uint32_t len);
265-
// ICM_20948_Status_e inv_icm20948_read_reg(ICM_20948_Device_t *pdev, uint8_t reg, uint8_t * buf, uint32_t len);
266-
// ICM_20948_Status_e inv_icm20948_serif_read_reg(ICM_20948_Device_t *pdev, uint8_t reg, uint8_t * buf, uint32_t len);
272+
ICM_20948_Status_e inv_icm20948_set_sensor_period(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, uint32_t period);
273+
ICM_20948_Status_e inv_icm20948_enable_sensor(ICM_20948_Device_t *pdev, enum inv_icm20948_sensor sensor, inv_bool_t state);
274+
static uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor);
275+
enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor);
267276

268277
// ToDo:
269278

0 commit comments

Comments
 (0)