test_code / Mbed OS test_icm20948

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@1:8459e28d77a1, 2021-03-29 (annotated)

Committer:
eric11fr
Date:
Mon Mar 29 21:16:25 2021 +0000
Revision:
1:8459e28d77a1
Parent:
0:efb1550773f1 
icm20948

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eric11fr 0:efb1550773f1 1 /* ICM20948 Basic Example Code
eric11fr 0:efb1550773f1 2 by: Kris Winer
eric11fr 1:8459e28d77a1 3 modified by Eric Nativel MBEB_OS6 port
eric11fr 1:8459e28d77a1 4 date: 29, MArch 2021
eric11fr 0:efb1550773f1 5 license: Beerware - Use this code however you'd like. If you
eric11fr 0:efb1550773f1 6 find it useful you can buy me a beer some time.
eric11fr 0:efb1550773f1 7 Modified by Brent Wilkins July 19, 2016
eric11fr 0:efb1550773f1 8 Demonstrate basic ICM20948 functionality including parameterizing the register
eric11fr 0:efb1550773f1 9 addresses, initializing the sensor, getting properly scaled accelerometer,
eric11fr 0:efb1550773f1 10 gyroscope, and magnetometer data out. Added display functions to allow display
eric11fr 0:efb1550773f1 11 to on breadboard monitor. Addition of 9 DoF sensor fusion using open source
eric11fr 1:8459e28d77a1 12 Madgwick and Mahony filter algorithms. Pimoroni icm20948 and stm32L432kc nucleo board
eric11fr 0:efb1550773f1 13 */
eric11fr 0:efb1550773f1 14
eric11fr 1:8459e28d77a1 15 #include "mbed.h"
eric11fr 1:8459e28d77a1 16 #include "ahrs.h"
eric11fr 1:8459e28d77a1 17 #include "icm20948.h"
eric11fr 1:8459e28d77a1 18 #include <cstdio>
eric11fr 1:8459e28d77a1 19 #include <stdint.h>
eric11fr 0:efb1550773f1 20
eric11fr 1:8459e28d77a1 21 using namespace std::chrono;
eric11fr 1:8459e28d77a1 22 Timer t1;
eric11fr 1:8459e28d77a1 23 typedef unsigned char byte;
eric11fr 1:8459e28d77a1 24 float selft[6];
eric11fr 1:8459e28d77a1 25 static BufferedSerial pc(USBTX, USBRX);
eric11fr 0:efb1550773f1 26
eric11fr 0:efb1550773f1 27
eric11fr 1:8459e28d77a1 28 char msg[255];
eric11fr 0:efb1550773f1 29
eric11fr 0:efb1550773f1 30 void setup()
eric11fr 0:efb1550773f1 31 {
eric11fr 1:8459e28d77a1 32 //Set up I2C
eric11fr 0:efb1550773f1 33
eric11fr 1:8459e28d77a1 34 pc.set_baud(9600);
eric11fr 1:8459e28d77a1 35 pc.set_format(
eric11fr 1:8459e28d77a1 36 /* bits */ 8,
eric11fr 1:8459e28d77a1 37 /* parity */ BufferedSerial::None,
eric11fr 1:8459e28d77a1 38 /* stop bit */ 1
eric11fr 1:8459e28d77a1 39 );
eric11fr 0:efb1550773f1 40 // Reset ICM20948
eric11fr 1:8459e28d77a1 41 begin();
eric11fr 1:8459e28d77a1 42
eric11fr 1:8459e28d77a1 43 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAGS);
eric11fr 1:8459e28d77a1 44 thread_sleep_for(100);
eric11fr 1:8459e28d77a1 45 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
eric11fr 1:8459e28d77a1 46 thread_sleep_for(100);
eric11fr 0:efb1550773f1 47
eric11fr 0:efb1550773f1 48 // Read the WHO_AM_I register, this is a good test of communication
eric11fr 1:8459e28d77a1 49 byte c = readByte(ICM20948_ADDRESS, WHO_AM_I_ICM20948);
eric11fr 1:8459e28d77a1 50 sprintf(msg,"ICM20948 I AM 0x %x I should be 0x %x",c,0xEA);
eric11fr 1:8459e28d77a1 51 pc.write(msg, strlen(msg));
eric11fr 0:efb1550773f1 52 if (c == 0xEA) // WHO_AM_I should always be 0x71
eric11fr 0:efb1550773f1 53 {
eric11fr 1:8459e28d77a1 54 sprintf(msg,"ICM20948 is online...\n");
eric11fr 1:8459e28d77a1 55 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 56 // writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
eric11fr 0:efb1550773f1 57 // Start by performing self test and reporting values
eric11fr 1:8459e28d77a1 58 ICM20948SelfTest(selft);
eric11fr 1:8459e28d77a1 59 sprintf(msg,"x-axis self test: acceleration trim within : %f of factory value\n",selft[0]);
eric11fr 1:8459e28d77a1 60 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 61 sprintf(msg,"y-axis self test: acceleration trim within : %f of factory value\n",selft[1]);
eric11fr 1:8459e28d77a1 62 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 63 sprintf(msg,"z-axis self test: acceleration trim within : %f of factory value\n",selft[2]);
eric11fr 1:8459e28d77a1 64 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 65 sprintf(msg,"x-axis self test: gyration trim within : %f of factory value\n",selft[3]);
eric11fr 1:8459e28d77a1 66 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 67 sprintf(msg,"y-axis self test: gyration trim within : %f of factory value\n",selft[4]);
eric11fr 1:8459e28d77a1 68 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 69 sprintf(msg,"z-axis self test: gyration trim within : %f of factory value\n",selft[5]);
eric11fr 1:8459e28d77a1 70 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 71 // Calibrate gyro and accelerometers, load biases in bias registers
eric11fr 1:8459e28d77a1 72 calibrateICM20948(gyroBias, accelBias);
eric11fr 0:efb1550773f1 73
eric11fr 1:8459e28d77a1 74 initICM20948();
eric11fr 0:efb1550773f1 75 // Initialize device for active mode read of acclerometer, gyroscope, and
eric11fr 0:efb1550773f1 76 // temperature
eric11fr 1:8459e28d77a1 77 sprintf(msg,"ICM20948 initialized for active data mode....\n");
eric11fr 1:8459e28d77a1 78 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 79 // Read the WHO_AM_I register of the magnetometer, this is a good test of
eric11fr 0:efb1550773f1 80 // communication
eric11fr 1:8459e28d77a1 81 tempCount =readTempData(); // Read the adc values
eric11fr 1:8459e28d77a1 82 // Temperature in degrees Centigrade
eric11fr 1:8459e28d77a1 83 temperature = ((float) tempCount) / 333.87 + 21.0;
eric11fr 1:8459e28d77a1 84 // Print temperature in degrees Centigrade
eric11fr 1:8459e28d77a1 85 sprintf(msg,"Temperature is %f degrees C\n",temperature);
eric11fr 1:8459e28d77a1 86 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 87 byte d = readByte(AK09916_ADDRESS<<1, WHO_AM_I_AK09916);
eric11fr 1:8459e28d77a1 88 sprintf(msg,"AK8963 I AM 0x %x I should be 0x %d\n",d,0x09);
eric11fr 1:8459e28d77a1 89 pc.write(msg, strlen(msg));
eric11fr 0:efb1550773f1 90
eric11fr 0:efb1550773f1 91 if (d != 0x09)
eric11fr 0:efb1550773f1 92 {
eric11fr 0:efb1550773f1 93 // Communication failed, stop here
eric11fr 1:8459e28d77a1 94 sprintf(msg,"Communication with magnetometer failed, abort!\n");
eric11fr 1:8459e28d77a1 95 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 96 exit(0);
eric11fr 0:efb1550773f1 97 }
eric11fr 0:efb1550773f1 98
eric11fr 0:efb1550773f1 99 // Get magnetometer calibration from AK8963 ROM
eric11fr 1:8459e28d77a1 100 initAK09916();
eric11fr 0:efb1550773f1 101 // Initialize device for active mode read of magnetometer
eric11fr 1:8459e28d77a1 102 sprintf(msg,"AK09916 initialized for active data mode....\n");
eric11fr 1:8459e28d77a1 103 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 104
eric11fr 1:8459e28d77a1 105
eric11fr 0:efb1550773f1 106
eric11fr 0:efb1550773f1 107 // Get sensor resolutions, only need to do this once
eric11fr 1:8459e28d77a1 108 getAres();
eric11fr 1:8459e28d77a1 109 getGres();
eric11fr 1:8459e28d77a1 110 getMres();
eric11fr 0:efb1550773f1 111 // The next call delays for 4 seconds, and then records about 15 seconds of
eric11fr 0:efb1550773f1 112 // data to calculate bias and scale.
eric11fr 1:8459e28d77a1 113 magCalICM20948(magBias, magScale);
eric11fr 1:8459e28d77a1 114 sprintf(msg,"AK09916 mag biases (mG)\n %f\n%f\n%f\n",magBias[0],magBias[1],magBias[2]);
eric11fr 1:8459e28d77a1 115 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 116 sprintf(msg,"AK09916 mag scale (mG)\n %f\n%f\n%f\n",magScale[0],magScale[1],magScale[2]);
eric11fr 1:8459e28d77a1 117 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 118 thread_sleep_for(2000); // Add delay to see results before pc spew of data
eric11fr 0:efb1550773f1 119 } // if (c == 0x71)
eric11fr 0:efb1550773f1 120 else
eric11fr 0:efb1550773f1 121 {
eric11fr 1:8459e28d77a1 122 sprintf(msg,"Could not connect to ICM20948: 0x%x",c);
eric11fr 1:8459e28d77a1 123 pc.write(msg, strlen(msg));
eric11fr 0:efb1550773f1 124 // Communication failed, stop here
eric11fr 1:8459e28d77a1 125 sprintf(msg," Communication failed, abort!\n");
eric11fr 1:8459e28d77a1 126 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 127 exit(0);
eric11fr 0:efb1550773f1 128 }
eric11fr 0:efb1550773f1 129 }
eric11fr 1:8459e28d77a1 130 int main(void)
eric11fr 1:8459e28d77a1 131 {int i=0;
eric11fr 1:8459e28d77a1 132 setup();
eric11fr 1:8459e28d77a1 133 while(i<100)
eric11fr 0:efb1550773f1 134 {
eric11fr 0:efb1550773f1 135 // If intPin goes high, all data registers have new data
eric11fr 0:efb1550773f1 136 // On interrupt, check if data ready interrupt
eric11fr 1:8459e28d77a1 137 if (readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01)
eric11fr 1:8459e28d77a1 138 {
eric11fr 1:8459e28d77a1 139 readAccelData(accelCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 140
eric11fr 0:efb1550773f1 141 // Now we'll calculate the accleration value into actual g's
eric11fr 0:efb1550773f1 142 // This depends on scale being set
eric11fr 1:8459e28d77a1 143 ax = (float)accelCount[0] * aRes; // - accelBias[0];
eric11fr 1:8459e28d77a1 144 ay = (float)accelCount[1] * aRes; // - accelBias[1];
eric11fr 1:8459e28d77a1 145 az = (float)accelCount[2] * aRes; // - accelBias[2];
eric11fr 1:8459e28d77a1 146 sprintf(msg,"X-acceleration: %f mg\n",1000*ax);
eric11fr 1:8459e28d77a1 147 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 148 sprintf(msg,"Y-acceleration: %f mg\n",1000*ay);
eric11fr 1:8459e28d77a1 149 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 150 sprintf(msg,"Z-acceleration: %f mg\n",1000*az);
eric11fr 1:8459e28d77a1 151 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 152 readGyroData(gyroCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 153
eric11fr 0:efb1550773f1 154 // Calculate the gyro value into actual degrees per second
eric11fr 0:efb1550773f1 155 // This depends on scale being set
eric11fr 1:8459e28d77a1 156 gx = (float)gyroCount[0] * gRes;
eric11fr 1:8459e28d77a1 157 gy = (float)gyroCount[1] * gRes;
eric11fr 1:8459e28d77a1 158 gz = (float)gyroCount[2] * gRes;
eric11fr 1:8459e28d77a1 159 sprintf(msg,"x -gyroscope: %f and bias %f deg/s\n",gx,gyroBias[0]);
eric11fr 1:8459e28d77a1 160 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 161 readMagData(magCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 162
eric11fr 0:efb1550773f1 163 // Calculate the magnetometer values in milliGauss
eric11fr 0:efb1550773f1 164 // Include factory calibration per data sheet and user environmental
eric11fr 0:efb1550773f1 165 // corrections
eric11fr 0:efb1550773f1 166 // Get actual magnetometer value, this depends on scale being set
eric11fr 1:8459e28d77a1 167 mx = (float)magCount[0] * mRes - magBias[0];
eric11fr 1:8459e28d77a1 168 my = (float)magCount[1] * mRes - magBias[1];
eric11fr 1:8459e28d77a1 169 mz = (float)magCount[2] * mRes - magBias[2];
eric11fr 1:8459e28d77a1 170 // if (readByte(ICM20948_ADDRESS, INT_STATUS) & 0x01)
eric11fr 0:efb1550773f1 171
eric11fr 0:efb1550773f1 172 // Must be called before updating quaternions!
eric11fr 1:8459e28d77a1 173 updateTime();
eric11fr 0:efb1550773f1 174
eric11fr 0:efb1550773f1 175 // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
eric11fr 0:efb1550773f1 176 // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
eric11fr 0:efb1550773f1 177 // (+ up) of accelerometer and gyro! We have to make some allowance for this
eric11fr 0:efb1550773f1 178 // orientationmismatch in feeding the output to the quaternion filter. For the
eric11fr 0:efb1550773f1 179 // ICM20948, we have chosen a magnetic rotation that keeps the sensor forward
eric11fr 0:efb1550773f1 180 // along the x-axis just like in the LSM9DS0 sensor. This rotation can be
eric11fr 0:efb1550773f1 181 // modified to allow any convenient orientation convention. This is ok by
eric11fr 0:efb1550773f1 182 // aircraft orientation standards! Pass gyro rate as rad/s
eric11fr 1:8459e28d77a1 183 MahonyQuaternionUpdate(ax, ay, az, gx * DEG_TO_RAD,
eric11fr 1:8459e28d77a1 184 gy * DEG_TO_RAD, gz * DEG_TO_RAD, my,
eric11fr 1:8459e28d77a1 185 mx, mz, deltat);
eric11fr 0:efb1550773f1 186
eric11fr 0:efb1550773f1 187 // Define output variables from updated quaternion---these are Tait-Bryan
eric11fr 0:efb1550773f1 188 // angles, commonly used in aircraft orientation. In this coordinate system,
eric11fr 0:efb1550773f1 189 // the positive z-axis is down toward Earth. Yaw is the angle between Sensor
eric11fr 0:efb1550773f1 190 // x-axis and Earth magnetic North (or true North if corrected for local
eric11fr 0:efb1550773f1 191 // declination, looking down on the sensor positive yaw is counterclockwise.
eric11fr 0:efb1550773f1 192 // Pitch is angle between sensor x-axis and Earth ground plane, toward the
eric11fr 0:efb1550773f1 193 // Earth is positive, up toward the sky is negative. Roll is angle between
eric11fr 0:efb1550773f1 194 // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
eric11fr 0:efb1550773f1 195 // arise from the definition of the homogeneous rotation matrix constructed
eric11fr 0:efb1550773f1 196 // from quaternions. Tait-Bryan angles as well as Euler angles are
eric11fr 0:efb1550773f1 197 // non-commutative; that is, the get the correct orientation the rotations
eric11fr 0:efb1550773f1 198 // must be applied in the correct order which for this configuration is yaw,
eric11fr 0:efb1550773f1 199 // pitch, and then roll.
eric11fr 0:efb1550773f1 200 // For more see
eric11fr 0:efb1550773f1 201 // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
eric11fr 0:efb1550773f1 202 // which has additional links.
eric11fr 1:8459e28d77a1 203 yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()
eric11fr 0:efb1550773f1 204 * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)
eric11fr 0:efb1550773f1 205 * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)
eric11fr 0:efb1550773f1 206 * *(getQ()+3));
eric11fr 1:8459e28d77a1 207 pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()
eric11fr 0:efb1550773f1 208 * *(getQ()+2)));
eric11fr 1:8459e28d77a1 209 roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2)
eric11fr 0:efb1550773f1 210 * *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1)
eric11fr 0:efb1550773f1 211 * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3)
eric11fr 0:efb1550773f1 212 * *(getQ()+3));
eric11fr 1:8459e28d77a1 213 pitch *= RAD_TO_DEG;
eric11fr 1:8459e28d77a1 214 yaw *= RAD_TO_DEG;
eric11fr 0:efb1550773f1 215
eric11fr 0:efb1550773f1 216 // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
eric11fr 0:efb1550773f1 217 // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
eric11fr 1:8459e28d77a1 218 // 1° 46' E 2021-03-27
eric11fr 0:efb1550773f1 219 // - http://www.ngdc.noaa.gov/geomag-web/#declination
eric11fr 1:8459e28d77a1 220 yaw -= 1.7666;
eric11fr 1:8459e28d77a1 221 roll *= RAD_TO_DEG;
eric11fr 0:efb1550773f1 222
eric11fr 1:8459e28d77a1 223
eric11fr 1:8459e28d77a1 224 sprintf(msg,"Yaw %f, Pitch %f, Roll %f\n ",yaw,pitch,roll);
eric11fr 1:8459e28d77a1 225 pc.write(msg, strlen(msg));
eric11fr 1:8459e28d77a1 226 sumCount = 0;
eric11fr 1:8459e28d77a1 227 sum = 0;
eric11fr 1:8459e28d77a1 228 }
eric11fr 1:8459e28d77a1 229 i++;
eric11fr 1:8459e28d77a1 230 //thread_sleep_for(200);
eric11fr 1:8459e28d77a1 231 }
eric11fr 1:8459e28d77a1 232 return 0;
eric11fr 0:efb1550773f1 233 }