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@0:efb1550773f1, 2021-03-19 (annotated)

Committer:
eric11fr
Date:
Fri Mar 19 21:59:25 2021 +0000
Revision:
0:efb1550773f1
Child:
1:8459e28d77a1
to

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 0:efb1550773f1 3 date: April 1, 2014
eric11fr 0:efb1550773f1 4 license: Beerware - Use this code however you'd like. If you
eric11fr 0:efb1550773f1 5 find it useful you can buy me a beer some time.
eric11fr 0:efb1550773f1 6 Modified by Brent Wilkins July 19, 2016
eric11fr 0:efb1550773f1 7 Demonstrate basic ICM20948 functionality including parameterizing the register
eric11fr 0:efb1550773f1 8 addresses, initializing the sensor, getting properly scaled accelerometer,
eric11fr 0:efb1550773f1 9 gyroscope, and magnetometer data out. Added display functions to allow display
eric11fr 0:efb1550773f1 10 to on breadboard monitor. Addition of 9 DoF sensor fusion using open source
eric11fr 0:efb1550773f1 11 Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini
eric11fr 0:efb1550773f1 12 and the Teensy 3.1.
eric11fr 0:efb1550773f1 13 SDA and SCL should have external pull-up resistors (to 3.3V).
eric11fr 0:efb1550773f1 14 10k resistors are on the EMSENSR-9250 breakout board.
eric11fr 0:efb1550773f1 15 Hardware setup:
eric11fr 0:efb1550773f1 16 ICM20948 Breakout --------- Arduino
eric11fr 0:efb1550773f1 17 VDD ---------------------- 3.3V
eric11fr 0:efb1550773f1 18 VDDI --------------------- 3.3V
eric11fr 0:efb1550773f1 19 SDA ----------------------- A4
eric11fr 0:efb1550773f1 20 SCL ----------------------- A5
eric11fr 0:efb1550773f1 21 GND ---------------------- GND
eric11fr 0:efb1550773f1 22 */
eric11fr 0:efb1550773f1 23
eric11fr 0:efb1550773f1 24 #include "AHRSAlgorithms.h"
eric11fr 0:efb1550773f1 25 #include "ICM20948.h"
eric11fr 0:efb1550773f1 26
eric11fr 0:efb1550773f1 27 #define AHRS false // Set to false for basic data read
eric11fr 0:efb1550773f1 28 #define SerialDebug true // Set to true to get Serial output for debugging
eric11fr 0:efb1550773f1 29
eric11fr 0:efb1550773f1 30
eric11fr 0:efb1550773f1 31 ICM20948 myIMU;
eric11fr 0:efb1550773f1 32
eric11fr 0:efb1550773f1 33 void setup()
eric11fr 0:efb1550773f1 34 {
eric11fr 0:efb1550773f1 35 Wire.begin();
eric11fr 0:efb1550773f1 36 // TWBR = 12; // 400 kbit/sec I2C speed
eric11fr 0:efb1550773f1 37 Serial.begin(115200);
eric11fr 0:efb1550773f1 38 while(!Serial) delay(10);
eric11fr 0:efb1550773f1 39
eric11fr 0:efb1550773f1 40 pinMode(myLed, OUTPUT);
eric11fr 0:efb1550773f1 41 digitalWrite(myLed, HIGH);
eric11fr 0:efb1550773f1 42
eric11fr 0:efb1550773f1 43 // Reset ICM20948
eric11fr 0:efb1550773f1 44 myIMU.writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAG);
eric11fr 0:efb1550773f1 45 delay(100);
eric11fr 0:efb1550773f1 46 myIMU.writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
eric11fr 0:efb1550773f1 47 delay(100);
eric11fr 0:efb1550773f1 48
eric11fr 0:efb1550773f1 49 // Read the WHO_AM_I register, this is a good test of communication
eric11fr 0:efb1550773f1 50 byte c = myIMU.readByte(ICM20948_ADDRESS, WHO_AM_I_ICM20948);
eric11fr 0:efb1550773f1 51 Serial.print(F("ICM20948 I AM 0x"));
eric11fr 0:efb1550773f1 52 Serial.print(c, HEX);
eric11fr 0:efb1550773f1 53 Serial.print(F(" I should be 0x"));
eric11fr 0:efb1550773f1 54 Serial.println(0xEA, HEX);
eric11fr 0:efb1550773f1 55
eric11fr 0:efb1550773f1 56 if (c == 0xEA) // WHO_AM_I should always be 0x71
eric11fr 0:efb1550773f1 57 {
eric11fr 0:efb1550773f1 58 Serial.println(F("ICM20948 is online..."));
eric11fr 0:efb1550773f1 59
eric11fr 0:efb1550773f1 60 // Start by performing self test and reporting values
eric11fr 0:efb1550773f1 61 myIMU.ICM20948SelfTest(myIMU.selfTest);
eric11fr 0:efb1550773f1 62 Serial.print(F("x-axis self test: acceleration trim within : "));
eric11fr 0:efb1550773f1 63 Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 64 Serial.print(F("y-axis self test: acceleration trim within : "));
eric11fr 0:efb1550773f1 65 Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 66 Serial.print(F("z-axis self test: acceleration trim within : "));
eric11fr 0:efb1550773f1 67 Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 68 Serial.print(F("x-axis self test: gyration trim within : "));
eric11fr 0:efb1550773f1 69 Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 70 Serial.print(F("y-axis self test: gyration trim within : "));
eric11fr 0:efb1550773f1 71 Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 72 Serial.print(F("z-axis self test: gyration trim within : "));
eric11fr 0:efb1550773f1 73 Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value");
eric11fr 0:efb1550773f1 74
eric11fr 0:efb1550773f1 75 // Calibrate gyro and accelerometers, load biases in bias registers
eric11fr 0:efb1550773f1 76 myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);
eric11fr 0:efb1550773f1 77
eric11fr 0:efb1550773f1 78 myIMU.initICM20948();
eric11fr 0:efb1550773f1 79 // Initialize device for active mode read of acclerometer, gyroscope, and
eric11fr 0:efb1550773f1 80 // temperature
eric11fr 0:efb1550773f1 81 Serial.println("ICM20948 initialized for active data mode....");
eric11fr 0:efb1550773f1 82
eric11fr 0:efb1550773f1 83 // Read the WHO_AM_I register of the magnetometer, this is a good test of
eric11fr 0:efb1550773f1 84 // communication
eric11fr 0:efb1550773f1 85 byte d = myIMU.readByte(AK09916_ADDRESS, WHO_AM_I_AK09916);
eric11fr 0:efb1550773f1 86 Serial.print("AK8963 ");
eric11fr 0:efb1550773f1 87 Serial.print("I AM 0x");
eric11fr 0:efb1550773f1 88 Serial.print(d, HEX);
eric11fr 0:efb1550773f1 89 Serial.print(" I should be 0x");
eric11fr 0:efb1550773f1 90 Serial.println(0x09, HEX);
eric11fr 0:efb1550773f1 91
eric11fr 0:efb1550773f1 92 if (d != 0x09)
eric11fr 0:efb1550773f1 93 {
eric11fr 0:efb1550773f1 94 // Communication failed, stop here
eric11fr 0:efb1550773f1 95 Serial.println(F("Communication with magnetometer failed, abort!"));
eric11fr 0:efb1550773f1 96 Serial.flush();
eric11fr 0:efb1550773f1 97 abort();
eric11fr 0:efb1550773f1 98 }
eric11fr 0:efb1550773f1 99
eric11fr 0:efb1550773f1 100 // Get magnetometer calibration from AK8963 ROM
eric11fr 0:efb1550773f1 101 myIMU.initAK09916();
eric11fr 0:efb1550773f1 102 // Initialize device for active mode read of magnetometer
eric11fr 0:efb1550773f1 103 Serial.println("AK09916 initialized for active data mode....");
eric11fr 0:efb1550773f1 104
eric11fr 0:efb1550773f1 105 /*
eric11fr 0:efb1550773f1 106 if (SerialDebug)
eric11fr 0:efb1550773f1 107 {
eric11fr 0:efb1550773f1 108 // Serial.println("Calibration values: ");
eric11fr 0:efb1550773f1 109 Serial.print("X-Axis factory sensitivity adjustment value ");
eric11fr 0:efb1550773f1 110 Serial.println(myIMU.factoryMagCalibration[0], 2);
eric11fr 0:efb1550773f1 111 Serial.print("Y-Axis factory sensitivity adjustment value ");
eric11fr 0:efb1550773f1 112 Serial.println(myIMU.factoryMagCalibration[1], 2);
eric11fr 0:efb1550773f1 113 Serial.print("Z-Axis factory sensitivity adjustment value ");
eric11fr 0:efb1550773f1 114 Serial.println(myIMU.factoryMagCalibration[2], 2);
eric11fr 0:efb1550773f1 115 }
eric11fr 0:efb1550773f1 116 */
eric11fr 0:efb1550773f1 117
eric11fr 0:efb1550773f1 118 // Get sensor resolutions, only need to do this once
eric11fr 0:efb1550773f1 119 myIMU.getAres();
eric11fr 0:efb1550773f1 120 myIMU.getGres();
eric11fr 0:efb1550773f1 121 myIMU.getMres();
eric11fr 0:efb1550773f1 122
eric11fr 0:efb1550773f1 123 // The next call delays for 4 seconds, and then records about 15 seconds of
eric11fr 0:efb1550773f1 124 // data to calculate bias and scale.
eric11fr 0:efb1550773f1 125 myIMU.magCalICM20948(myIMU.magBias, myIMU.magScale);
eric11fr 0:efb1550773f1 126 Serial.println("AK09916 mag biases (mG)");
eric11fr 0:efb1550773f1 127 Serial.println(myIMU.magBias[0]);
eric11fr 0:efb1550773f1 128 Serial.println(myIMU.magBias[1]);
eric11fr 0:efb1550773f1 129 Serial.println(myIMU.magBias[2]);
eric11fr 0:efb1550773f1 130
eric11fr 0:efb1550773f1 131 Serial.println("AK09916 mag scale (mG)");
eric11fr 0:efb1550773f1 132 Serial.println(myIMU.magScale[0]);
eric11fr 0:efb1550773f1 133 Serial.println(myIMU.magScale[1]);
eric11fr 0:efb1550773f1 134 Serial.println(myIMU.magScale[2]);
eric11fr 0:efb1550773f1 135 delay(2000); // Add delay to see results before serial spew of data
eric11fr 0:efb1550773f1 136 } // if (c == 0x71)
eric11fr 0:efb1550773f1 137 else
eric11fr 0:efb1550773f1 138 {
eric11fr 0:efb1550773f1 139 Serial.print("Could not connect to ICM20948: 0x");
eric11fr 0:efb1550773f1 140 Serial.println(c, HEX);
eric11fr 0:efb1550773f1 141
eric11fr 0:efb1550773f1 142 // Communication failed, stop here
eric11fr 0:efb1550773f1 143 Serial.println(F("Communication failed, abort!"));
eric11fr 0:efb1550773f1 144 Serial.flush();
eric11fr 0:efb1550773f1 145 abort();
eric11fr 0:efb1550773f1 146 }
eric11fr 0:efb1550773f1 147 }
eric11fr 0:efb1550773f1 148
eric11fr 0:efb1550773f1 149 void loop()
eric11fr 0:efb1550773f1 150 {
eric11fr 0:efb1550773f1 151 // If intPin goes high, all data registers have new data
eric11fr 0:efb1550773f1 152 // On interrupt, check if data ready interrupt
eric11fr 0:efb1550773f1 153 if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01)
eric11fr 0:efb1550773f1 154 {
eric11fr 0:efb1550773f1 155 myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 156
eric11fr 0:efb1550773f1 157 // Now we'll calculate the accleration value into actual g's
eric11fr 0:efb1550773f1 158 // This depends on scale being set
eric11fr 0:efb1550773f1 159 myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0];
eric11fr 0:efb1550773f1 160 myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1];
eric11fr 0:efb1550773f1 161 myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2];
eric11fr 0:efb1550773f1 162
eric11fr 0:efb1550773f1 163 myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 164
eric11fr 0:efb1550773f1 165 // Calculate the gyro value into actual degrees per second
eric11fr 0:efb1550773f1 166 // This depends on scale being set
eric11fr 0:efb1550773f1 167 myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
eric11fr 0:efb1550773f1 168 myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
eric11fr 0:efb1550773f1 169 myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
eric11fr 0:efb1550773f1 170
eric11fr 0:efb1550773f1 171 myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
eric11fr 0:efb1550773f1 172
eric11fr 0:efb1550773f1 173 // Calculate the magnetometer values in milliGauss
eric11fr 0:efb1550773f1 174 // Include factory calibration per data sheet and user environmental
eric11fr 0:efb1550773f1 175 // corrections
eric11fr 0:efb1550773f1 176 // Get actual magnetometer value, this depends on scale being set
eric11fr 0:efb1550773f1 177 myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes - myIMU.magBias[0];
eric11fr 0:efb1550773f1 178 myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes - myIMU.magBias[1];
eric11fr 0:efb1550773f1 179 myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes - myIMU.magBias[2];
eric11fr 0:efb1550773f1 180 } // if (readByte(ICM20948_ADDRESS, INT_STATUS) & 0x01)
eric11fr 0:efb1550773f1 181
eric11fr 0:efb1550773f1 182 // Must be called before updating quaternions!
eric11fr 0:efb1550773f1 183 myIMU.updateTime();
eric11fr 0:efb1550773f1 184
eric11fr 0:efb1550773f1 185 // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
eric11fr 0:efb1550773f1 186 // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
eric11fr 0:efb1550773f1 187 // (+ up) of accelerometer and gyro! We have to make some allowance for this
eric11fr 0:efb1550773f1 188 // orientationmismatch in feeding the output to the quaternion filter. For the
eric11fr 0:efb1550773f1 189 // ICM20948, we have chosen a magnetic rotation that keeps the sensor forward
eric11fr 0:efb1550773f1 190 // along the x-axis just like in the LSM9DS0 sensor. This rotation can be
eric11fr 0:efb1550773f1 191 // modified to allow any convenient orientation convention. This is ok by
eric11fr 0:efb1550773f1 192 // aircraft orientation standards! Pass gyro rate as rad/s
eric11fr 0:efb1550773f1 193 MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
eric11fr 0:efb1550773f1 194 myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
eric11fr 0:efb1550773f1 195 myIMU.mx, myIMU.mz, myIMU.deltat);
eric11fr 0:efb1550773f1 196
eric11fr 0:efb1550773f1 197 if (!AHRS)
eric11fr 0:efb1550773f1 198 {
eric11fr 0:efb1550773f1 199 myIMU.delt_t = millis() - myIMU.count;
eric11fr 0:efb1550773f1 200 if (myIMU.delt_t > 500)
eric11fr 0:efb1550773f1 201 {
eric11fr 0:efb1550773f1 202 if(SerialDebug)
eric11fr 0:efb1550773f1 203 {
eric11fr 0:efb1550773f1 204 // Print acceleration values in milligs!
eric11fr 0:efb1550773f1 205 Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax);
eric11fr 0:efb1550773f1 206 Serial.print(" mg ");
eric11fr 0:efb1550773f1 207 Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay);
eric11fr 0:efb1550773f1 208 Serial.print(" mg ");
eric11fr 0:efb1550773f1 209 Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az);
eric11fr 0:efb1550773f1 210 Serial.println(" mg ");
eric11fr 0:efb1550773f1 211
eric11fr 0:efb1550773f1 212 // Print gyro values in degree/sec
eric11fr 0:efb1550773f1 213 Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3);
eric11fr 0:efb1550773f1 214 Serial.print(" degrees/sec ");
eric11fr 0:efb1550773f1 215 Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3);
eric11fr 0:efb1550773f1 216 Serial.print(" degrees/sec ");
eric11fr 0:efb1550773f1 217 Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3);
eric11fr 0:efb1550773f1 218 Serial.println(" degrees/sec");
eric11fr 0:efb1550773f1 219
eric11fr 0:efb1550773f1 220 // Print mag values in degree/sec
eric11fr 0:efb1550773f1 221 Serial.print("X-mag field: "); Serial.print(myIMU.mx);
eric11fr 0:efb1550773f1 222 Serial.print(" mG ");
eric11fr 0:efb1550773f1 223 Serial.print("Y-mag field: "); Serial.print(myIMU.my);
eric11fr 0:efb1550773f1 224 Serial.print(" mG ");
eric11fr 0:efb1550773f1 225 Serial.print("Z-mag field: "); Serial.print(myIMU.mz);
eric11fr 0:efb1550773f1 226 Serial.println(" mG");
eric11fr 0:efb1550773f1 227
eric11fr 0:efb1550773f1 228 myIMU.tempCount = myIMU.readTempData(); // Read the adc values
eric11fr 0:efb1550773f1 229 // Temperature in degrees Centigrade
eric11fr 0:efb1550773f1 230 myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0;
eric11fr 0:efb1550773f1 231 // Print temperature in degrees Centigrade
eric11fr 0:efb1550773f1 232 Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1);
eric11fr 0:efb1550773f1 233 Serial.println(" degrees C");
eric11fr 0:efb1550773f1 234 }
eric11fr 0:efb1550773f1 235
eric11fr 0:efb1550773f1 236 myIMU.count = millis();
eric11fr 0:efb1550773f1 237 digitalWrite(myLed, !digitalRead(myLed)); // toggle led
eric11fr 0:efb1550773f1 238 } // if (myIMU.delt_t > 500)
eric11fr 0:efb1550773f1 239 } // if (!AHRS)
eric11fr 0:efb1550773f1 240 else
eric11fr 0:efb1550773f1 241 {
eric11fr 0:efb1550773f1 242 // Serial print and/or display at 0.5 s rate independent of data rates
eric11fr 0:efb1550773f1 243 myIMU.delt_t = millis() - myIMU.count;
eric11fr 0:efb1550773f1 244
eric11fr 0:efb1550773f1 245 // update LCD once per half-second independent of read rate
eric11fr 0:efb1550773f1 246 if (myIMU.delt_t > 500)
eric11fr 0:efb1550773f1 247 {
eric11fr 0:efb1550773f1 248 if(SerialDebug)
eric11fr 0:efb1550773f1 249 {
eric11fr 0:efb1550773f1 250 Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax);
eric11fr 0:efb1550773f1 251 Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay);
eric11fr 0:efb1550773f1 252 Serial.print(" az = "); Serial.print((int)1000 * myIMU.az);
eric11fr 0:efb1550773f1 253 Serial.println(" mg");
eric11fr 0:efb1550773f1 254
eric11fr 0:efb1550773f1 255 Serial.print("gx = "); Serial.print(myIMU.gx, 2);
eric11fr 0:efb1550773f1 256 Serial.print(" gy = "); Serial.print(myIMU.gy, 2);
eric11fr 0:efb1550773f1 257 Serial.print(" gz = "); Serial.print(myIMU.gz, 2);
eric11fr 0:efb1550773f1 258 Serial.println(" deg/s");
eric11fr 0:efb1550773f1 259
eric11fr 0:efb1550773f1 260 Serial.print("mx = "); Serial.print((int)myIMU.mx);
eric11fr 0:efb1550773f1 261 Serial.print(" my = "); Serial.print((int)myIMU.my);
eric11fr 0:efb1550773f1 262 Serial.print(" mz = "); Serial.print((int)myIMU.mz);
eric11fr 0:efb1550773f1 263 Serial.println(" mG");
eric11fr 0:efb1550773f1 264
eric11fr 0:efb1550773f1 265 Serial.print("q0 = "); Serial.print(*getQ());
eric11fr 0:efb1550773f1 266 Serial.print(" qx = "); Serial.print(*(getQ() + 1));
eric11fr 0:efb1550773f1 267 Serial.print(" qy = "); Serial.print(*(getQ() + 2));
eric11fr 0:efb1550773f1 268 Serial.print(" qz = "); Serial.println(*(getQ() + 3));
eric11fr 0:efb1550773f1 269 }
eric11fr 0:efb1550773f1 270
eric11fr 0:efb1550773f1 271 // Define output variables from updated quaternion---these are Tait-Bryan
eric11fr 0:efb1550773f1 272 // angles, commonly used in aircraft orientation. In this coordinate system,
eric11fr 0:efb1550773f1 273 // the positive z-axis is down toward Earth. Yaw is the angle between Sensor
eric11fr 0:efb1550773f1 274 // x-axis and Earth magnetic North (or true North if corrected for local
eric11fr 0:efb1550773f1 275 // declination, looking down on the sensor positive yaw is counterclockwise.
eric11fr 0:efb1550773f1 276 // Pitch is angle between sensor x-axis and Earth ground plane, toward the
eric11fr 0:efb1550773f1 277 // Earth is positive, up toward the sky is negative. Roll is angle between
eric11fr 0:efb1550773f1 278 // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
eric11fr 0:efb1550773f1 279 // arise from the definition of the homogeneous rotation matrix constructed
eric11fr 0:efb1550773f1 280 // from quaternions. Tait-Bryan angles as well as Euler angles are
eric11fr 0:efb1550773f1 281 // non-commutative; that is, the get the correct orientation the rotations
eric11fr 0:efb1550773f1 282 // must be applied in the correct order which for this configuration is yaw,
eric11fr 0:efb1550773f1 283 // pitch, and then roll.
eric11fr 0:efb1550773f1 284 // For more see
eric11fr 0:efb1550773f1 285 // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
eric11fr 0:efb1550773f1 286 // which has additional links.
eric11fr 0:efb1550773f1 287 myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()
eric11fr 0:efb1550773f1 288 * *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)
eric11fr 0:efb1550773f1 289 * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)
eric11fr 0:efb1550773f1 290 * *(getQ()+3));
eric11fr 0:efb1550773f1 291 myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()
eric11fr 0:efb1550773f1 292 * *(getQ()+2)));
eric11fr 0:efb1550773f1 293 myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2)
eric11fr 0:efb1550773f1 294 * *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1)
eric11fr 0:efb1550773f1 295 * *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3)
eric11fr 0:efb1550773f1 296 * *(getQ()+3));
eric11fr 0:efb1550773f1 297 myIMU.pitch *= RAD_TO_DEG;
eric11fr 0:efb1550773f1 298 myIMU.yaw *= RAD_TO_DEG;
eric11fr 0:efb1550773f1 299
eric11fr 0:efb1550773f1 300 // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
eric11fr 0:efb1550773f1 301 // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
eric11fr 0:efb1550773f1 302 // - http://www.ngdc.noaa.gov/geomag-web/#declination
eric11fr 0:efb1550773f1 303 myIMU.yaw -= 8.5;
eric11fr 0:efb1550773f1 304 myIMU.roll *= RAD_TO_DEG;
eric11fr 0:efb1550773f1 305
eric11fr 0:efb1550773f1 306 if(SerialDebug)
eric11fr 0:efb1550773f1 307 {
eric11fr 0:efb1550773f1 308 Serial.print("Yaw, Pitch, Roll: ");
eric11fr 0:efb1550773f1 309 Serial.print(myIMU.yaw, 2);
eric11fr 0:efb1550773f1 310 Serial.print(", ");
eric11fr 0:efb1550773f1 311 Serial.print(myIMU.pitch, 2);
eric11fr 0:efb1550773f1 312 Serial.print(", ");
eric11fr 0:efb1550773f1 313 Serial.println(myIMU.roll, 2);
eric11fr 0:efb1550773f1 314
eric11fr 0:efb1550773f1 315 Serial.print("rate = ");
eric11fr 0:efb1550773f1 316 Serial.print((float)myIMU.sumCount / myIMU.sum, 2);
eric11fr 0:efb1550773f1 317 Serial.println(" Hz");
eric11fr 0:efb1550773f1 318 }
eric11fr 0:efb1550773f1 319
eric11fr 0:efb1550773f1 320 myIMU.count = millis();
eric11fr 0:efb1550773f1 321 myIMU.sumCount = 0;
eric11fr 0:efb1550773f1 322 myIMU.sum = 0;
eric11fr 0:efb1550773f1 323 } // if (myIMU.delt_t > 500)
eric11fr 0:efb1550773f1 324 } // if (AHRS)
eric11fr 0:efb1550773f1 325 }