Skip to content

Commit d5cada9

Browse files
PaulZCPaulZC
committed
Starting to add DMP functionality. Firmware upload seems to work!
1 parent 8402d6f commit d5cada9

File tree

9 files changed

+1364
-1
lines changed

9 files changed

+1364
-1
lines changed

‎examples/Arduino/Example5_DMP/Example5_DMP.ino‎

Lines changed: 171 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
1+
/****************************************************************
2+
* Example5_DMP.ino
3+
* ICM 20948 Arduino Library Demo
4+
* Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
5+
* Paul Clark, February 9th 2021
6+
* Based on original code by:
7+
* Owen Lyke @ SparkFun Electronics
8+
* Original Creation Date: April 17 2019
9+
*
10+
* Please see License.md for the license information.
11+
*
12+
* Distributed as-is; no warranty is given.
13+
***************************************************************/
14+
#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
15+
16+
//#define USE_SPI // Uncomment this to use SPI
17+
18+
#define SERIAL_PORT Serial
19+
20+
#define SPI_PORT SPI // Your desired SPI port. Used only when "USE_SPI" is defined
21+
#define CS_PIN 2 // Which pin you connect CS to. Used only when "USE_SPI" is defined
22+
23+
#define WIRE_PORT Wire // Your desired Wire port. Used when "USE_SPI" is not defined
24+
#define AD0_VAL 1 // The value of the last bit of the I2C address.
25+
// On the SparkFun 9DoF IMU breakout the default is 1, and when
26+
// the ADR jumper is closed the value becomes 0
27+
28+
#ifdef USE_SPI
29+
ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
30+
#else
31+
ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
32+
#endif
33+
34+
35+
void setup() {
36+
37+
SERIAL_PORT.begin(115200);
38+
while(!SERIAL_PORT){};
39+
40+
#ifdef USE_SPI
41+
SPI_PORT.begin();
42+
#else
43+
WIRE_PORT.begin();
44+
WIRE_PORT.setClock(400000);
45+
#endif
46+
47+
myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
48+
49+
bool initialized = false;
50+
while( !initialized ){
51+
52+
#ifdef USE_SPI
53+
myICM.begin( CS_PIN, SPI_PORT );
54+
#else
55+
myICM.begin( WIRE_PORT, AD0_VAL );
56+
#endif
57+
58+
SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
59+
SERIAL_PORT.println( myICM.statusString() );
60+
if( myICM.status != ICM_20948_Stat_Ok ){
61+
SERIAL_PORT.println( "Trying again..." );
62+
delay(500);
63+
}else{
64+
initialized = true;
65+
}
66+
}
67+
68+
}
69+
70+
void loop() {
71+
72+
}
73+
74+
75+
// Below here are some helper functions to print the data nicely!
76+
77+
void printPaddedInt16b( int16_t val ){
78+
if(val > 0){
79+
SERIAL_PORT.print(" ");
80+
if(val < 10000){ SERIAL_PORT.print("0"); }
81+
if(val < 1000 ){ SERIAL_PORT.print("0"); }
82+
if(val < 100 ){ SERIAL_PORT.print("0"); }
83+
if(val < 10 ){ SERIAL_PORT.print("0"); }
84+
}else{
85+
SERIAL_PORT.print("-");
86+
if(abs(val) < 10000){ SERIAL_PORT.print("0"); }
87+
if(abs(val) < 1000 ){ SERIAL_PORT.print("0"); }
88+
if(abs(val) < 100 ){ SERIAL_PORT.print("0"); }
89+
if(abs(val) < 10 ){ SERIAL_PORT.print("0"); }
90+
}
91+
SERIAL_PORT.print(abs(val));
92+
}
93+
94+
void printRawAGMT( ICM_20948_AGMT_t agmt){
95+
SERIAL_PORT.print("RAW. Acc [ ");
96+
printPaddedInt16b( agmt.acc.axes.x );
97+
SERIAL_PORT.print(", ");
98+
printPaddedInt16b( agmt.acc.axes.y );
99+
SERIAL_PORT.print(", ");
100+
printPaddedInt16b( agmt.acc.axes.z );
101+
SERIAL_PORT.print(" ], Gyr [ ");
102+
printPaddedInt16b( agmt.gyr.axes.x );
103+
SERIAL_PORT.print(", ");
104+
printPaddedInt16b( agmt.gyr.axes.y );
105+
SERIAL_PORT.print(", ");
106+
printPaddedInt16b( agmt.gyr.axes.z );
107+
SERIAL_PORT.print(" ], Mag [ ");
108+
printPaddedInt16b( agmt.mag.axes.x );
109+
SERIAL_PORT.print(", ");
110+
printPaddedInt16b( agmt.mag.axes.y );
111+
SERIAL_PORT.print(", ");
112+
printPaddedInt16b( agmt.mag.axes.z );
113+
SERIAL_PORT.print(" ], Tmp [ ");
114+
printPaddedInt16b( agmt.tmp.val );
115+
SERIAL_PORT.print(" ]");
116+
SERIAL_PORT.println();
117+
}
118+
119+
120+
void printFormattedFloat(float val, uint8_t leading, uint8_t decimals){
121+
float aval = abs(val);
122+
if(val < 0){
123+
SERIAL_PORT.print("-");
124+
}else{
125+
SERIAL_PORT.print(" ");
126+
}
127+
for( uint8_t indi = 0; indi < leading; indi++ ){
128+
uint32_t tenpow = 0;
129+
if( indi < (leading-1) ){
130+
tenpow = 1;
131+
}
132+
for(uint8_t c = 0; c < (leading-1-indi); c++){
133+
tenpow *= 10;
134+
}
135+
if( aval < tenpow){
136+
SERIAL_PORT.print("0");
137+
}else{
138+
break;
139+
}
140+
}
141+
if(val < 0){
142+
SERIAL_PORT.print(-val, decimals);
143+
}else{
144+
SERIAL_PORT.print(val, decimals);
145+
}
146+
}
147+
148+
void printScaledAGMT( ICM_20948_AGMT_t agmt){
149+
SERIAL_PORT.print("Scaled. Acc (mg) [ ");
150+
printFormattedFloat( myICM.accX(), 5, 2 );
151+
SERIAL_PORT.print(", ");
152+
printFormattedFloat( myICM.accY(), 5, 2 );
153+
SERIAL_PORT.print(", ");
154+
printFormattedFloat( myICM.accZ(), 5, 2 );
155+
SERIAL_PORT.print(" ], Gyr (DPS) [ ");
156+
printFormattedFloat( myICM.gyrX(), 5, 2 );
157+
SERIAL_PORT.print(", ");
158+
printFormattedFloat( myICM.gyrY(), 5, 2 );
159+
SERIAL_PORT.print(", ");
160+
printFormattedFloat( myICM.gyrZ(), 5, 2 );
161+
SERIAL_PORT.print(" ], Mag (uT) [ ");
162+
printFormattedFloat( myICM.magX(), 5, 2 );
163+
SERIAL_PORT.print(", ");
164+
printFormattedFloat( myICM.magY(), 5, 2 );
165+
SERIAL_PORT.print(", ");
166+
printFormattedFloat( myICM.magZ(), 5, 2 );
167+
SERIAL_PORT.print(" ], Tmp (C) [ ");
168+
printFormattedFloat( myICM.temp(), 5, 2 );
169+
SERIAL_PORT.print(" ]");
170+
SERIAL_PORT.println();
171+
}

‎src/ICM_20948.cpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -777,6 +777,16 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
777777
return status;
778778
}
779779

780+
retval = loadDMPFirmware();
781+
if (retval != ICM_20948_Stat_Ok)
782+
{
783+
debugPrint(F("ICM_20948::startupDefault: loadDMPFirmware returned: "));
784+
debugPrintStatus(retval);
785+
debugPrintln(F(""));
786+
status = retval;
787+
return status;
788+
}
789+
780790
retval = swReset();
781791
if (retval != ICM_20948_Stat_Ok)
782792
{
@@ -900,6 +910,14 @@ ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
900910
return status;
901911
}
902912

913+
// DMP
914+
915+
ICM_20948_Status_e ICM_20948::loadDMPFirmware(void)
916+
{
917+
status = ICM_20948_firmware_load(&_device);
918+
return status;
919+
}
920+
903921
// I2C
904922
ICM_20948_I2C::ICM_20948_I2C()
905923
{
@@ -940,6 +958,8 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
940958
// Link the serif
941959
_device._serif = &_serif;
942960

961+
_device._firmware_loaded = false; // Initialize _firmware_loaded
962+
943963
// Perform default startup
944964
status = startupDefault();
945965
if (status != ICM_20948_Stat_Ok)
@@ -1110,6 +1130,8 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
11101130
// Link the serif
11111131
_device._serif = &_serif;
11121132

1133+
_device._firmware_loaded = false; // Initialize _firmware_loaded
1134+
11131135
// Perform default startup
11141136
status = startupDefault();
11151137
if (status != ICM_20948_Stat_Ok)

‎src/ICM_20948.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -162,6 +162,9 @@ class ICM_20948
162162
ICM_20948_Status_e magWhoIAm(void);
163163
uint8_t readMag(AK09916_Reg_Addr_e reg);
164164
ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata);
165+
166+
//DMP
167+
ICM_20948_Status_e loadDMPFirmware(void);
165168
};
166169

167170
// I2C

0 commit comments

Comments
 (0)