Kris Winer / MPU9250AHRS_nRF52832

Added mag calibration and interrupt-based data ready

Dependencies:   BLE_API mbed-src nRF51822

main.cpp@4:8d11bfc7cac0, 2016-09-22 (annotated)

Committer:
onehorse
Date:
Thu Sep 22 01:21:24 2016 +0000
Revision:
4:8d11bfc7cac0
Parent:
3:fe46f14f5aef 
Added mag calibration and interrupt-based data ready

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 0:2e5e65a6fb30 1 /* MPU9250 Basic Example Code
onehorse 0:2e5e65a6fb30 2 by: Kris Winer
onehorse 4:8d11bfc7cac0 3 date: September 20, 2016
onehorse 0:2e5e65a6fb30 4 license: Beerware - Use this code however you'd like. If you
onehorse 0:2e5e65a6fb30 5 find it useful you can buy me a beer some time.
onehorse 0:2e5e65a6fb30 6
onehorse 0:2e5e65a6fb30 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
onehorse 0:2e5e65a6fb30 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
onehorse 0:2e5e65a6fb30 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
onehorse 4:8d11bfc7cac0 10 Mahony filter algorithms.
onehorse 0:2e5e65a6fb30 11
onehorse 0:2e5e65a6fb30 12 SDA and SCL should have external pull-up resistors (to 3.3V).
onehorse 0:2e5e65a6fb30 13 */
onehorse 0:2e5e65a6fb30 14
onehorse 0:2e5e65a6fb30 15 #include "mbed.h"
onehorse 0:2e5e65a6fb30 16 #include "MPU9250.h"
onehorse 4:8d11bfc7cac0 17 #include "BMP280.h"
onehorse 4:8d11bfc7cac0 18 #include "math.h"
onehorse 0:2e5e65a6fb30 19
onehorse 4:8d11bfc7cac0 20 MPU9250 mpu9250; // Instantiate MPU9250 class
onehorse 4:8d11bfc7cac0 21
onehorse 4:8d11bfc7cac0 22 BMP280 bmp280; // Instantiate BMP280 class
onehorse 4:8d11bfc7cac0 23
onehorse 4:8d11bfc7cac0 24 Timer t;
onehorse 4:8d11bfc7cac0 25
onehorse 4:8d11bfc7cac0 26 InterruptIn myInterrupt(P0_8); // One nRF52 Dev Board variant uses pin 8, one uses pin 10
onehorse 4:8d11bfc7cac0 27
onehorse 4:8d11bfc7cac0 28 /* Serial pc(USBTX, USBRX); // tx, rx*/
onehorse 4:8d11bfc7cac0 29 Serial pc(P0_12, P0_14); // tx, rx
onehorse 0:2e5e65a6fb30 30
onehorse 0:2e5e65a6fb30 31 float sum = 0;
onehorse 0:2e5e65a6fb30 32 uint32_t sumCount = 0;
onehorse 0:2e5e65a6fb30 33 char buffer[14];
onehorse 4:8d11bfc7cac0 34 uint8_t whoami = 0;
onehorse 4:8d11bfc7cac0 35 double Temperature, Pressure; // stores BMP280 pressures sensor pressure and temperature
onehorse 4:8d11bfc7cac0 36 int32_t rawPress, rawTemp; // pressure and temperature raw count output for BMP280
onehorse 0:2e5e65a6fb30 37
onehorse 4:8d11bfc7cac0 38 int32_t readBMP280Temperature()
onehorse 4:8d11bfc7cac0 39 {
onehorse 4:8d11bfc7cac0 40 uint8_t rawData[3]; // 20-bit pressure register data stored here
onehorse 4:8d11bfc7cac0 41 bmp280.readBytes(BMP280_ADDRESS, BMP280_TEMP_MSB, 3, &rawData[0]);
onehorse 4:8d11bfc7cac0 42 return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
onehorse 4:8d11bfc7cac0 43 }
onehorse 4:8d11bfc7cac0 44
onehorse 4:8d11bfc7cac0 45 int32_t readBMP280Pressure()
onehorse 4:8d11bfc7cac0 46 {
onehorse 4:8d11bfc7cac0 47 uint8_t rawData[3]; // 20-bit pressure register data stored here
onehorse 4:8d11bfc7cac0 48 bmp280.readBytes(BMP280_ADDRESS, BMP280_PRESS_MSB, 3, &rawData[0]);
onehorse 4:8d11bfc7cac0 49 return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
onehorse 4:8d11bfc7cac0 50 }
onehorse 4:8d11bfc7cac0 51
onehorse 4:8d11bfc7cac0 52 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of
onehorse 4:8d11bfc7cac0 53 // “5123” equals 51.23 DegC.
onehorse 4:8d11bfc7cac0 54 int32_t bmp280_compensate_T(int32_t adc_T)
onehorse 4:8d11bfc7cac0 55 {
onehorse 4:8d11bfc7cac0 56 int32_t var1, var2, T;
onehorse 4:8d11bfc7cac0 57 var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
onehorse 4:8d11bfc7cac0 58 var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
onehorse 4:8d11bfc7cac0 59 t_fine = var1 + var2;
onehorse 4:8d11bfc7cac0 60 T = (t_fine * 5 + 128) >> 8;
onehorse 4:8d11bfc7cac0 61 return T;
onehorse 4:8d11bfc7cac0 62 }
onehorse 0:2e5e65a6fb30 63
onehorse 4:8d11bfc7cac0 64 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
onehorse 4:8d11bfc7cac0 65 //fractional bits).
onehorse 4:8d11bfc7cac0 66 //Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
onehorse 4:8d11bfc7cac0 67 uint32_t bmp280_compensate_P(int32_t adc_P)
onehorse 4:8d11bfc7cac0 68 {
onehorse 4:8d11bfc7cac0 69 long long var1, var2, p;
onehorse 4:8d11bfc7cac0 70 var1 = ((long long)t_fine) - 128000;
onehorse 4:8d11bfc7cac0 71 var2 = var1 * var1 * (long long)dig_P6;
onehorse 4:8d11bfc7cac0 72 var2 = var2 + ((var1*(long long)dig_P5)<<17);
onehorse 4:8d11bfc7cac0 73 var2 = var2 + (((long long)dig_P4)<<35);
onehorse 4:8d11bfc7cac0 74 var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);
onehorse 4:8d11bfc7cac0 75 var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;
onehorse 4:8d11bfc7cac0 76 if(var1 == 0)
onehorse 4:8d11bfc7cac0 77 {
onehorse 4:8d11bfc7cac0 78 return 0;
onehorse 4:8d11bfc7cac0 79 // avoid exception caused by division by zero
onehorse 4:8d11bfc7cac0 80 }
onehorse 4:8d11bfc7cac0 81 p = 1048576 - adc_P;
onehorse 4:8d11bfc7cac0 82 p = (((p<<31) - var2)*3125)/var1;
onehorse 4:8d11bfc7cac0 83 var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;
onehorse 4:8d11bfc7cac0 84 var2 = (((long long)dig_P8) * p)>> 19;
onehorse 4:8d11bfc7cac0 85 p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);
onehorse 4:8d11bfc7cac0 86 return (uint32_t)p;
onehorse 4:8d11bfc7cac0 87 }
onehorse 0:2e5e65a6fb30 88
onehorse 4:8d11bfc7cac0 89 void myinthandler() // interrupt handler
onehorse 4:8d11bfc7cac0 90 {
onehorse 4:8d11bfc7cac0 91 newData = true;
onehorse 4:8d11bfc7cac0 92 }
onehorse 4:8d11bfc7cac0 93
onehorse 0:2e5e65a6fb30 94
onehorse 0:2e5e65a6fb30 95 int main()
onehorse 0:2e5e65a6fb30 96 {
onehorse 0:2e5e65a6fb30 97 pc.baud(9600);
onehorse 4:8d11bfc7cac0 98 myled = 0; // turn off led
onehorse 4:8d11bfc7cac0 99
onehorse 4:8d11bfc7cac0 100 wait(5);
onehorse 4:8d11bfc7cac0 101
onehorse 0:2e5e65a6fb30 102 //Set up I2C
onehorse 0:2e5e65a6fb30 103 i2c.frequency(400000); // use fast (400 kHz) I2C
onehorse 4:8d11bfc7cac0 104
onehorse 4:8d11bfc7cac0 105 t.start(); // enable system timer
onehorse 0:2e5e65a6fb30 106
onehorse 4:8d11bfc7cac0 107 myled = 1; // turn on led
onehorse 4:8d11bfc7cac0 108
onehorse 4:8d11bfc7cac0 109 myInterrupt.rise(&myinthandler); // define interrupt for INT pin output of MPU9250
onehorse 0:2e5e65a6fb30 110
onehorse 4:8d11bfc7cac0 111 // Read the WHO_AM_I register, this is a good test of communication
onehorse 4:8d11bfc7cac0 112 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
onehorse 4:8d11bfc7cac0 113 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
onehorse 4:8d11bfc7cac0 114 myled = 1;
onehorse 0:2e5e65a6fb30 115
onehorse 4:8d11bfc7cac0 116 if (whoami == 0x71) // WHO_AM_I should always be 0x71
onehorse 0:2e5e65a6fb30 117 {
onehorse 0:2e5e65a6fb30 118 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
onehorse 0:2e5e65a6fb30 119 pc.printf("MPU9250 is online...\n\r");
onehorse 0:2e5e65a6fb30 120 wait(1);
onehorse 0:2e5e65a6fb30 121
onehorse 0:2e5e65a6fb30 122 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
onehorse 4:8d11bfc7cac0 123
onehorse 1:71c319f03fda 124 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
onehorse 4:8d11bfc7cac0 125 pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[0]);
onehorse 4:8d11bfc7cac0 126 pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
onehorse 4:8d11bfc7cac0 127 pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
onehorse 4:8d11bfc7cac0 128 pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
onehorse 4:8d11bfc7cac0 129 pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
onehorse 4:8d11bfc7cac0 130 pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
onehorse 4:8d11bfc7cac0 131
onehorse 4:8d11bfc7cac0 132 mpu9250.getAres(); // Get accelerometer sensitivity
onehorse 4:8d11bfc7cac0 133 mpu9250.getGres(); // Get gyro sensitivity
onehorse 4:8d11bfc7cac0 134 mpu9250.getMres(); // Get magnetometer sensitivity
onehorse 4:8d11bfc7cac0 135 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
onehorse 4:8d11bfc7cac0 136 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
onehorse 4:8d11bfc7cac0 137 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
onehorse 4:8d11bfc7cac0 138
onehorse 0:2e5e65a6fb30 139 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
onehorse 0:2e5e65a6fb30 140 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
onehorse 0:2e5e65a6fb30 141 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
onehorse 0:2e5e65a6fb30 142 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
onehorse 0:2e5e65a6fb30 143 pc.printf("x accel bias = %f\n\r", accelBias[0]);
onehorse 0:2e5e65a6fb30 144 pc.printf("y accel bias = %f\n\r", accelBias[1]);
onehorse 0:2e5e65a6fb30 145 pc.printf("z accel bias = %f\n\r", accelBias[2]);
onehorse 0:2e5e65a6fb30 146 wait(2);
onehorse 4:8d11bfc7cac0 147
onehorse 0:2e5e65a6fb30 148 mpu9250.initMPU9250();
onehorse 0:2e5e65a6fb30 149 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
onehorse 4:8d11bfc7cac0 150 wait(1);
onehorse 4:8d11bfc7cac0 151
onehorse 0:2e5e65a6fb30 152 mpu9250.initAK8963(magCalibration);
onehorse 0:2e5e65a6fb30 153 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
onehorse 0:2e5e65a6fb30 154 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
onehorse 0:2e5e65a6fb30 155 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
onehorse 0:2e5e65a6fb30 156 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
onehorse 0:2e5e65a6fb30 157 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
onehorse 0:2e5e65a6fb30 158 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
onehorse 0:2e5e65a6fb30 159 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
onehorse 4:8d11bfc7cac0 160
onehorse 4:8d11bfc7cac0 161 pc.printf("Mag Calibration: Wave device in a figure eight until done!");
onehorse 4:8d11bfc7cac0 162 wait(4);
onehorse 4:8d11bfc7cac0 163 mpu9250.magcalMPU9250(magBias, magScale);
onehorse 4:8d11bfc7cac0 164 pc.printf("Mag Calibration done!\n\r");
onehorse 4:8d11bfc7cac0 165 pc.printf("x mag bias = %f\n\r", magBias[0]);
onehorse 4:8d11bfc7cac0 166 pc.printf("y mag bias = %f\n\r", magBias[1]);
onehorse 4:8d11bfc7cac0 167 pc.printf("z mag bias = %f\n\r", magBias[2]);
onehorse 4:8d11bfc7cac0 168 wait(2);
onehorse 4:8d11bfc7cac0 169 }
onehorse 4:8d11bfc7cac0 170
onehorse 0:2e5e65a6fb30 171 else
onehorse 4:8d11bfc7cac0 172
onehorse 0:2e5e65a6fb30 173 {
onehorse 0:2e5e65a6fb30 174 pc.printf("Could not connect to MPU9250: \n\r");
onehorse 0:2e5e65a6fb30 175 pc.printf("%#x \n", whoami);
onehorse 4:8d11bfc7cac0 176 myled = 0;
onehorse 0:2e5e65a6fb30 177
onehorse 0:2e5e65a6fb30 178 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:2e5e65a6fb30 179 }
onehorse 4:8d11bfc7cac0 180
onehorse 4:8d11bfc7cac0 181 // Read the WHO_AM_I register of the BMP-280, this is a good test of communication
onehorse 4:8d11bfc7cac0 182 uint8_t c = bmp280.readByte(BMP280_ADDRESS, BMP280_ID);
onehorse 4:8d11bfc7cac0 183 if(c == 0x58) {
onehorse 4:8d11bfc7cac0 184
onehorse 4:8d11bfc7cac0 185 pc.printf("BMP-280 is 0x%x\n\r", c);
onehorse 4:8d11bfc7cac0 186 pc.printf("BMP-280 should be 0x58\n\r");
onehorse 4:8d11bfc7cac0 187 pc.printf("BMP-280 online...\n\r");
onehorse 4:8d11bfc7cac0 188
onehorse 4:8d11bfc7cac0 189 //bmp280.BMP280Init();
onehorse 4:8d11bfc7cac0 190
onehorse 4:8d11bfc7cac0 191 // Set T and P oversampling rates and sensor mode
onehorse 4:8d11bfc7cac0 192 bmp280.writeByte(BMP280_ADDRESS, BMP280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
onehorse 4:8d11bfc7cac0 193 // Set standby time interval in normal mode and bandwidth
onehorse 4:8d11bfc7cac0 194 bmp280.writeByte(BMP280_ADDRESS, BMP280_CONFIG, SBy << 5 | IIRFilter << 2);
onehorse 4:8d11bfc7cac0 195 uint8_t calib[24];
onehorse 4:8d11bfc7cac0 196 bmp280.readBytes(BMP280_ADDRESS, BMP280_CALIB00, 24, &calib[0]);
onehorse 4:8d11bfc7cac0 197 dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
onehorse 4:8d11bfc7cac0 198 dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
onehorse 4:8d11bfc7cac0 199 dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
onehorse 4:8d11bfc7cac0 200 dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
onehorse 4:8d11bfc7cac0 201 dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
onehorse 4:8d11bfc7cac0 202 dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
onehorse 4:8d11bfc7cac0 203 dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
onehorse 4:8d11bfc7cac0 204 dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
onehorse 4:8d11bfc7cac0 205 dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
onehorse 4:8d11bfc7cac0 206 dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
onehorse 4:8d11bfc7cac0 207 dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
onehorse 4:8d11bfc7cac0 208 dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
onehorse 0:2e5e65a6fb30 209
onehorse 4:8d11bfc7cac0 210 pc.printf("dig_T1 is %d\n\r", dig_T1);
onehorse 4:8d11bfc7cac0 211 pc.printf("dig_T2 is %d\n\r", dig_T2);
onehorse 4:8d11bfc7cac0 212 pc.printf("dig_T3 is %d\n\r", dig_T3);
onehorse 4:8d11bfc7cac0 213 pc.printf("dig_P1 is %d\n\r", dig_P1);
onehorse 4:8d11bfc7cac0 214 pc.printf("dig_P2 is %d\n\r", dig_P2);
onehorse 4:8d11bfc7cac0 215 pc.printf("dig_P3 is %d\n\r", dig_P3);
onehorse 4:8d11bfc7cac0 216 pc.printf("dig_P4 is %d\n\r", dig_P4);
onehorse 4:8d11bfc7cac0 217 pc.printf("dig_P5 is %d\n\r", dig_P5);
onehorse 4:8d11bfc7cac0 218 pc.printf("dig_P6 is %d\n\r", dig_P6);
onehorse 4:8d11bfc7cac0 219 pc.printf("dig_P7 is %d\n\r", dig_P7);
onehorse 4:8d11bfc7cac0 220 pc.printf("dig_P8 is %d\n\r", dig_P8);
onehorse 4:8d11bfc7cac0 221 pc.printf("dig_P9 is %d\n\r", dig_P9);
onehorse 4:8d11bfc7cac0 222
onehorse 4:8d11bfc7cac0 223 pc.printf("BMP-280 calibration complete...\n\r");
onehorse 4:8d11bfc7cac0 224
onehorse 4:8d11bfc7cac0 225 }
onehorse 4:8d11bfc7cac0 226
onehorse 4:8d11bfc7cac0 227 else
onehorse 4:8d11bfc7cac0 228
onehorse 4:8d11bfc7cac0 229 {
onehorse 4:8d11bfc7cac0 230 pc.printf("BMP-280 is 0x%x\n\r", c);
onehorse 4:8d11bfc7cac0 231 pc.printf("BMP-280 should be 0x55\n\r");
onehorse 4:8d11bfc7cac0 232 while(1); // idle here forever
onehorse 4:8d11bfc7cac0 233 }
onehorse 4:8d11bfc7cac0 234
onehorse 4:8d11bfc7cac0 235 /* Main Loop*/
onehorse 0:2e5e65a6fb30 236 while(1) {
onehorse 0:2e5e65a6fb30 237
onehorse 0:2e5e65a6fb30 238 // If intPin goes high, all data registers have new data
onehorse 4:8d11bfc7cac0 239 // if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // OUse polling to check for data ready
onehorse 4:8d11bfc7cac0 240 if(newData){ // wait for interrupt for data ready
onehorse 4:8d11bfc7cac0 241 newData = false; // reset newData flag
onehorse 4:8d11bfc7cac0 242
onehorse 4:8d11bfc7cac0 243 mpu9250.readMPU9250Data(MPU9250Data); // INT cleared on any read
onehorse 0:2e5e65a6fb30 244
onehorse 4:8d11bfc7cac0 245 // mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
onehorse 0:2e5e65a6fb30 246 // Now we'll calculate the accleration value into actual g's
onehorse 4:8d11bfc7cac0 247 ax = (float)MPU9250Data[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
onehorse 4:8d11bfc7cac0 248 ay = (float)MPU9250Data[1]*aRes - accelBias[1];
onehorse 4:8d11bfc7cac0 249 az = (float)MPU9250Data[2]*aRes - accelBias[2];
onehorse 4:8d11bfc7cac0 250
onehorse 4:8d11bfc7cac0 251 // mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
onehorse 0:2e5e65a6fb30 252 // Calculate the gyro value into actual degrees per second
onehorse 4:8d11bfc7cac0 253 gx = (float)MPU9250Data[4]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
onehorse 4:8d11bfc7cac0 254 gy = (float)MPU9250Data[5]*gRes - gyroBias[1];
onehorse 4:8d11bfc7cac0 255 gz = (float)MPU9250Data[6]*gRes - gyroBias[2];
onehorse 0:2e5e65a6fb30 256
onehorse 4:8d11bfc7cac0 257 }
onehorse 4:8d11bfc7cac0 258
onehorse 4:8d11bfc7cac0 259 if(mpu9250.readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) {
onehorse 4:8d11bfc7cac0 260
onehorse 4:8d11bfc7cac0 261 mpu9250.readMagData(magCount); // Read the x/y/z adc values
onehorse 0:2e5e65a6fb30 262 // Calculate the magnetometer values in milliGauss
onehorse 0:2e5e65a6fb30 263 // Include factory calibration per data sheet and user environmental corrections
onehorse 4:8d11bfc7cac0 264 mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0]; // get actual magnetometer value, this depends on scale being set
onehorse 4:8d11bfc7cac0 265 my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];
onehorse 4:8d11bfc7cac0 266 mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];
onehorse 4:8d11bfc7cac0 267 mx *= magScale[0]; // poor man's soft iron calibration
onehorse 4:8d11bfc7cac0 268 my *= magScale[1];
onehorse 4:8d11bfc7cac0 269 mz *= magScale[2];
onehorse 4:8d11bfc7cac0 270 }
onehorse 0:2e5e65a6fb30 271
onehorse 0:2e5e65a6fb30 272 Now = t.read_us();
onehorse 0:2e5e65a6fb30 273 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
onehorse 0:2e5e65a6fb30 274 lastUpdate = Now;
onehorse 0:2e5e65a6fb30 275
onehorse 0:2e5e65a6fb30 276 sum += deltat;
onehorse 0:2e5e65a6fb30 277 sumCount++;
onehorse 0:2e5e65a6fb30 278
onehorse 0:2e5e65a6fb30 279 // Pass gyro rate as rad/s
onehorse 4:8d11bfc7cac0 280 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
onehorse 4:8d11bfc7cac0 281 // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
onehorse 0:2e5e65a6fb30 282
onehorse 4:8d11bfc7cac0 283 // Serial print and/or display at 1 s rate independent of data rates
onehorse 0:2e5e65a6fb30 284 delt_t = t.read_ms() - count;
onehorse 4:8d11bfc7cac0 285 if (delt_t > 1000) { // update LCD once per second independent of read rate
onehorse 0:2e5e65a6fb30 286
onehorse 0:2e5e65a6fb30 287 pc.printf("ax = %f", 1000*ax);
onehorse 0:2e5e65a6fb30 288 pc.printf(" ay = %f", 1000*ay);
onehorse 0:2e5e65a6fb30 289 pc.printf(" az = %f mg\n\r", 1000*az);
onehorse 0:2e5e65a6fb30 290
onehorse 0:2e5e65a6fb30 291 pc.printf("gx = %f", gx);
onehorse 0:2e5e65a6fb30 292 pc.printf(" gy = %f", gy);
onehorse 0:2e5e65a6fb30 293 pc.printf(" gz = %f deg/s\n\r", gz);
onehorse 0:2e5e65a6fb30 294
onehorse 4:8d11bfc7cac0 295 pc.printf("mx = %f", mx);
onehorse 4:8d11bfc7cac0 296 pc.printf(" my = %f", my);
onehorse 4:8d11bfc7cac0 297 pc.printf(" mz = %f mG\n\r", mz);
onehorse 0:2e5e65a6fb30 298
onehorse 4:8d11bfc7cac0 299 // tempCount = mpu9250.readTempData(); // Read the adc values
onehorse 4:8d11bfc7cac0 300 temperature = ((float) MPU9250Data[3]) / 333.87f + 21.0f; // Temperature in degrees Centigrade
onehorse 4:8d11bfc7cac0 301 pc.printf("gyro temperature = %f C\n\r", temperature);
onehorse 4:8d11bfc7cac0 302
onehorse 4:8d11bfc7cac0 303 pc.printf("q0, q1, q2, q3 = %f %f %f %f\n\r",q[0], q[1], q[2], q[3]);
onehorse 0:2e5e65a6fb30 304
onehorse 4:8d11bfc7cac0 305 rawPress = readBMP280Pressure();
onehorse 4:8d11bfc7cac0 306 Pressure = (float) bmp280_compensate_P(rawPress)/25600.0f; // Pressure in mbar
onehorse 4:8d11bfc7cac0 307 rawTemp = readBMP280Temperature();
onehorse 4:8d11bfc7cac0 308 Temperature = (float) bmp280_compensate_T(rawTemp)/100.0f;
onehorse 4:8d11bfc7cac0 309
onehorse 4:8d11bfc7cac0 310 float altitude = 145366.45f*(1.0f - powf(Pressure/1013.25f, 0.190284f) );
onehorse 4:8d11bfc7cac0 311 pc.printf("Temperature = %f C\n\r", Temperature);
onehorse 4:8d11bfc7cac0 312 pc.printf("Pressure = %f Pa\n\r", Pressure);
onehorse 4:8d11bfc7cac0 313 pc.printf("Altitude = %f feet\n\r", altitude);
onehorse 4:8d11bfc7cac0 314
onehorse 0:2e5e65a6fb30 315 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
onehorse 0:2e5e65a6fb30 316 // In this coordinate system, the positive z-axis is down toward Earth.
onehorse 0:2e5e65a6fb30 317 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
onehorse 0:2e5e65a6fb30 318 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
onehorse 0:2e5e65a6fb30 319 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
onehorse 0:2e5e65a6fb30 320 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
onehorse 0:2e5e65a6fb30 321 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
onehorse 0:2e5e65a6fb30 322 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
onehorse 0:2e5e65a6fb30 323 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
onehorse 4:8d11bfc7cac0 324 yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
onehorse 4:8d11bfc7cac0 325 pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
onehorse 4:8d11bfc7cac0 326 roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
onehorse 0:2e5e65a6fb30 327 pitch *= 180.0f / PI;
onehorse 0:2e5e65a6fb30 328 yaw *= 180.0f / PI;
onehorse 4:8d11bfc7cac0 329 yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
onehorse 0:2e5e65a6fb30 330 roll *= 180.0f / PI;
onehorse 0:2e5e65a6fb30 331
onehorse 0:2e5e65a6fb30 332 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
onehorse 4:8d11bfc7cac0 333 pc.printf("average rate = %f Hz \n\r", (float) sumCount/sum);
onehorse 0:2e5e65a6fb30 334
onehorse 0:2e5e65a6fb30 335 myled= !myled;
onehorse 0:2e5e65a6fb30 336 count = t.read_ms();
onehorse 0:2e5e65a6fb30 337
onehorse 0:2e5e65a6fb30 338 if(count > 1<<21) {
onehorse 0:2e5e65a6fb30 339 t.start(); // start the timer over again if ~30 minutes has passed
onehorse 0:2e5e65a6fb30 340 count = 0;
onehorse 0:2e5e65a6fb30 341 deltat= 0;
onehorse 0:2e5e65a6fb30 342 lastUpdate = t.read_us();
onehorse 0:2e5e65a6fb30 343 }
onehorse 0:2e5e65a6fb30 344 sum = 0;
onehorse 0:2e5e65a6fb30 345 sumCount = 0;
onehorse 0:2e5e65a6fb30 346 }
onehorse 4:8d11bfc7cac0 347
onehorse 0:2e5e65a6fb30 348 }
onehorse 0:2e5e65a6fb30 349
onehorse 4:8d11bfc7cac0 350 }