Sensor data verwerking
Dependencies: SEGGER_RTT mbed
Fork of MPU9250AHRS by
main.cpp@3:d53674889db3, 2018-02-19 (annotated)
- Committer:
- MarijnJ
- Date:
- Mon Feb 19 12:43:16 2018 +0000
- Revision:
- 3:d53674889db3
- Parent:
- 1:71c319f03fda
Simpele sensor data verwerking zonder callibratie
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
onehorse | 0:2e5e65a6fb30 | 1 | /* MPU9250 Basic Example Code |
MarijnJ | 3:d53674889db3 | 2 | by: Kris Winer |
MarijnJ | 3:d53674889db3 | 3 | date: April 1, 2014 |
MarijnJ | 3:d53674889db3 | 4 | license: Beerware - Use this code however you'd like. If you |
MarijnJ | 3:d53674889db3 | 5 | find it useful you can buy me a beer some time. |
MarijnJ | 3:d53674889db3 | 6 | |
MarijnJ | 3:d53674889db3 | 7 | Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, |
MarijnJ | 3:d53674889db3 | 8 | getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to |
MarijnJ | 3:d53674889db3 | 9 | allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and |
MarijnJ | 3:d53674889db3 | 10 | Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. |
MarijnJ | 3:d53674889db3 | 11 | |
MarijnJ | 3:d53674889db3 | 12 | ----------------------------------------------------------------------- |
MarijnJ | 3:d53674889db3 | 13 | |
MarijnJ | 3:d53674889db3 | 14 | Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822 |
MarijnJ | 3:d53674889db3 | 15 | date: February 16, 2018 |
MarijnJ | 3:d53674889db3 | 16 | */ |
MarijnJ | 3:d53674889db3 | 17 | |
onehorse | 0:2e5e65a6fb30 | 18 | #include "mbed.h" |
onehorse | 0:2e5e65a6fb30 | 19 | #include "MPU9250.h" |
MarijnJ | 3:d53674889db3 | 20 | #include "SEGGER_RTT.h" |
MarijnJ | 3:d53674889db3 | 21 | #include "SEGGER_RTT.c" |
MarijnJ | 3:d53674889db3 | 22 | #include "SEGGER_RTT_printf.c" |
onehorse | 0:2e5e65a6fb30 | 23 | |
MarijnJ | 3:d53674889db3 | 24 | float sum = 0, shift = 0; |
onehorse | 0:2e5e65a6fb30 | 25 | uint32_t sumCount = 0; |
MarijnJ | 3:d53674889db3 | 26 | int cycle = 0; |
onehorse | 0:2e5e65a6fb30 | 27 | char buffer[14]; |
onehorse | 0:2e5e65a6fb30 | 28 | |
MarijnJ | 3:d53674889db3 | 29 | MPU9250 mpu9250; |
onehorse | 0:2e5e65a6fb30 | 30 | |
MarijnJ | 3:d53674889db3 | 31 | Timer t; |
onehorse | 0:2e5e65a6fb30 | 32 | |
MarijnJ | 3:d53674889db3 | 33 | short toInt(float x) |
MarijnJ | 3:d53674889db3 | 34 | { |
MarijnJ | 3:d53674889db3 | 35 | return (x >= 0) ? (int)(x + 0.5) : (int)(x - 0.5); |
MarijnJ | 3:d53674889db3 | 36 | } |
MarijnJ | 3:d53674889db3 | 37 | int getTime(int counter) |
MarijnJ | 3:d53674889db3 | 38 | { |
MarijnJ | 3:d53674889db3 | 39 | return (int)((counter/60000.0f)+shift); |
MarijnJ | 3:d53674889db3 | 40 | } |
MarijnJ | 3:d53674889db3 | 41 | |
onehorse | 0:2e5e65a6fb30 | 42 | int main() |
onehorse | 0:2e5e65a6fb30 | 43 | { |
MarijnJ | 3:d53674889db3 | 44 | //Set up I2C |
MarijnJ | 3:d53674889db3 | 45 | i2c.frequency(400000); // use fast (400 kHz) I2C |
MarijnJ | 3:d53674889db3 | 46 | SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); |
MarijnJ | 3:d53674889db3 | 47 | t.start(); |
onehorse | 0:2e5e65a6fb30 | 48 | |
MarijnJ | 3:d53674889db3 | 49 | // Read the WHO_AM_I register, this is a good test of communication |
MarijnJ | 3:d53674889db3 | 50 | uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
onehorse | 0:2e5e65a6fb30 | 51 | |
MarijnJ | 3:d53674889db3 | 52 | if (whoami == 0x71) // WHO_AM_I should always be 0x68 |
MarijnJ | 3:d53674889db3 | 53 | { |
MarijnJ | 3:d53674889db3 | 54 | SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n"); |
MarijnJ | 3:d53674889db3 | 55 | wait(1); |
MarijnJ | 3:d53674889db3 | 56 | |
MarijnJ | 3:d53674889db3 | 57 | mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration |
MarijnJ | 3:d53674889db3 | 58 | mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values |
MarijnJ | 3:d53674889db3 | 59 | SEGGER_RTT_printf(0, "x-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[0])); |
MarijnJ | 3:d53674889db3 | 60 | SEGGER_RTT_printf(0, "y-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[1])); |
MarijnJ | 3:d53674889db3 | 61 | SEGGER_RTT_printf(0, "z-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[2])); |
MarijnJ | 3:d53674889db3 | 62 | SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[3])); |
MarijnJ | 3:d53674889db3 | 63 | SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[4])); |
MarijnJ | 3:d53674889db3 | 64 | SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n", toInt(SelfTest[5])); |
MarijnJ | 3:d53674889db3 | 65 | |
MarijnJ | 3:d53674889db3 | 66 | mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
MarijnJ | 3:d53674889db3 | 67 | SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(gyroBias[0])); |
MarijnJ | 3:d53674889db3 | 68 | SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(gyroBias[1])); |
MarijnJ | 3:d53674889db3 | 69 | SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(gyroBias[2])); |
MarijnJ | 3:d53674889db3 | 70 | SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(accelBias[0])); |
MarijnJ | 3:d53674889db3 | 71 | SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(accelBias[1])); |
MarijnJ | 3:d53674889db3 | 72 | SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(accelBias[2])); |
MarijnJ | 3:d53674889db3 | 73 | wait(2); |
MarijnJ | 3:d53674889db3 | 74 | |
MarijnJ | 3:d53674889db3 | 75 | mpu9250.initMPU9250(); |
MarijnJ | 3:d53674889db3 | 76 | SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
MarijnJ | 3:d53674889db3 | 77 | mpu9250.initAK8963(magCalibration); |
MarijnJ | 3:d53674889db3 | 78 | SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer |
MarijnJ | 3:d53674889db3 | 79 | SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n", toInt(2.0f*(float)(1<<Ascale))); |
MarijnJ | 3:d53674889db3 | 80 | SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n", toInt(250.0f*(float)(1<<Gscale))); |
MarijnJ | 3:d53674889db3 | 81 | if(Mscale == 0) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n"); |
MarijnJ | 3:d53674889db3 | 82 | if(Mscale == 1) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n"); |
MarijnJ | 3:d53674889db3 | 83 | if(Mmode == 2) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n"); |
MarijnJ | 3:d53674889db3 | 84 | if(Mmode == 6) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n"); |
MarijnJ | 3:d53674889db3 | 85 | SEGGER_RTT_WriteString(0, "\n"); |
MarijnJ | 3:d53674889db3 | 86 | wait(1); |
MarijnJ | 3:d53674889db3 | 87 | } |
MarijnJ | 3:d53674889db3 | 88 | else { |
MarijnJ | 3:d53674889db3 | 89 | SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami); |
MarijnJ | 3:d53674889db3 | 90 | while(1); // Loop forever if communication doesn't happen |
onehorse | 0:2e5e65a6fb30 | 91 | } |
onehorse | 0:2e5e65a6fb30 | 92 | |
onehorse | 0:2e5e65a6fb30 | 93 | mpu9250.getAres(); // Get accelerometer sensitivity |
onehorse | 0:2e5e65a6fb30 | 94 | mpu9250.getGres(); // Get gyro sensitivity |
onehorse | 0:2e5e65a6fb30 | 95 | mpu9250.getMres(); // Get magnetometer sensitivity |
MarijnJ | 3:d53674889db3 | 96 | SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n", toInt(1.0f/aRes)); |
MarijnJ | 3:d53674889db3 | 97 | SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n", toInt(1.0f/gRes)); |
MarijnJ | 3:d53674889db3 | 98 | SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n", toInt(1.0f/mRes)); |
onehorse | 0:2e5e65a6fb30 | 99 | magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated |
onehorse | 0:2e5e65a6fb30 | 100 | magbias[1] = +120.; // User environmental x-axis correction in milliGauss |
onehorse | 0:2e5e65a6fb30 | 101 | magbias[2] = +125.; // User environmental x-axis correction in milliGauss |
onehorse | 0:2e5e65a6fb30 | 102 | |
MarijnJ | 3:d53674889db3 | 103 | while(1) |
MarijnJ | 3:d53674889db3 | 104 | { |
MarijnJ | 3:d53674889db3 | 105 | // If intPin goes high, all data registers have new data |
MarijnJ | 3:d53674889db3 | 106 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) // On interrupt, check if data ready interrupt |
MarijnJ | 3:d53674889db3 | 107 | { |
MarijnJ | 3:d53674889db3 | 108 | mpu9250.readAccelData(accelCount); // Read the x/y/z adc values |
MarijnJ | 3:d53674889db3 | 109 | // Now we'll calculate the accleration value into actual g's |
MarijnJ | 3:d53674889db3 | 110 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
MarijnJ | 3:d53674889db3 | 111 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
MarijnJ | 3:d53674889db3 | 112 | az = (float)accelCount[2]*aRes - accelBias[2]; |
MarijnJ | 3:d53674889db3 | 113 | |
MarijnJ | 3:d53674889db3 | 114 | mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values |
MarijnJ | 3:d53674889db3 | 115 | // Calculate the gyro value into actual degrees per second |
MarijnJ | 3:d53674889db3 | 116 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
MarijnJ | 3:d53674889db3 | 117 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
MarijnJ | 3:d53674889db3 | 118 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
MarijnJ | 3:d53674889db3 | 119 | |
MarijnJ | 3:d53674889db3 | 120 | mpu9250.readMagData(magCount); // Read the x/y/z adc values |
MarijnJ | 3:d53674889db3 | 121 | // Calculate the magnetometer values in milliGauss |
MarijnJ | 3:d53674889db3 | 122 | // Include factory calibration per data sheet and user environmental corrections |
MarijnJ | 3:d53674889db3 | 123 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set |
MarijnJ | 3:d53674889db3 | 124 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
MarijnJ | 3:d53674889db3 | 125 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
MarijnJ | 3:d53674889db3 | 126 | } |
onehorse | 0:2e5e65a6fb30 | 127 | |
MarijnJ | 3:d53674889db3 | 128 | Now = t.read_us(); |
MarijnJ | 3:d53674889db3 | 129 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
MarijnJ | 3:d53674889db3 | 130 | lastUpdate = Now; |
MarijnJ | 3:d53674889db3 | 131 | |
MarijnJ | 3:d53674889db3 | 132 | sum += deltat; |
MarijnJ | 3:d53674889db3 | 133 | sumCount++; |
MarijnJ | 3:d53674889db3 | 134 | |
MarijnJ | 3:d53674889db3 | 135 | if(lastUpdate - firstUpdate > 10000000.0f) |
MarijnJ | 3:d53674889db3 | 136 | { |
MarijnJ | 3:d53674889db3 | 137 | beta = 0.04; // decrease filter gain after stabilized |
MarijnJ | 3:d53674889db3 | 138 | zeta = 0.015; // increasey bias drift gain after stabilized |
MarijnJ | 3:d53674889db3 | 139 | } |
MarijnJ | 3:d53674889db3 | 140 | |
MarijnJ | 3:d53674889db3 | 141 | // Pass gyro rate as rad/s |
MarijnJ | 3:d53674889db3 | 142 | // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
MarijnJ | 3:d53674889db3 | 143 | mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
onehorse | 0:2e5e65a6fb30 | 144 | |
MarijnJ | 3:d53674889db3 | 145 | // Serial print 1 s rate independent of data rates |
MarijnJ | 3:d53674889db3 | 146 | delt_t = t.read_ms() - count; |
MarijnJ | 3:d53674889db3 | 147 | if (delt_t > 1000) // update print once per second independent of read rate |
MarijnJ | 3:d53674889db3 | 148 | { |
MarijnJ | 3:d53674889db3 | 149 | SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000*ax)); |
MarijnJ | 3:d53674889db3 | 150 | SEGGER_RTT_printf(0, " ay = %d", toInt(1000*ay)); |
MarijnJ | 3:d53674889db3 | 151 | SEGGER_RTT_printf(0, " az = %d mg\n", toInt(1000*az)); |
MarijnJ | 3:d53674889db3 | 152 | |
MarijnJ | 3:d53674889db3 | 153 | SEGGER_RTT_printf(0, "gx = %d", toInt(gx)); |
MarijnJ | 3:d53674889db3 | 154 | SEGGER_RTT_printf(0, " gy = %d", toInt(gy)); |
MarijnJ | 3:d53674889db3 | 155 | SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(gz)); |
MarijnJ | 3:d53674889db3 | 156 | |
MarijnJ | 3:d53674889db3 | 157 | SEGGER_RTT_printf(0, "mx = %d", toInt(mx)); |
MarijnJ | 3:d53674889db3 | 158 | SEGGER_RTT_printf(0, " my = %d", toInt(my)); |
MarijnJ | 3:d53674889db3 | 159 | SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(mz)); |
MarijnJ | 3:d53674889db3 | 160 | |
MarijnJ | 3:d53674889db3 | 161 | tempCount = mpu9250.readTempData(); // Read the adc values |
MarijnJ | 3:d53674889db3 | 162 | temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Celsius |
MarijnJ | 3:d53674889db3 | 163 | SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(temperature)); |
MarijnJ | 3:d53674889db3 | 164 | |
MarijnJ | 3:d53674889db3 | 165 | //SEGGER_RTT_printf(0, "q0 = %d\n", toInt(q[0]*100)); |
MarijnJ | 3:d53674889db3 | 166 | //SEGGER_RTT_printf(0, "q1 = %d\n", toInt(q[1]*100)); |
MarijnJ | 3:d53674889db3 | 167 | //SEGGER_RTT_printf(0, "q2 = %d\n", toInt(q[2]*100)); |
MarijnJ | 3:d53674889db3 | 168 | //SEGGER_RTT_printf(0, "q3 = %d\n", toInt(q[3]*100)); |
MarijnJ | 3:d53674889db3 | 169 | |
MarijnJ | 3:d53674889db3 | 170 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
MarijnJ | 3:d53674889db3 | 171 | // In this coordinate system, the positive z-axis is down toward Earth. |
MarijnJ | 3:d53674889db3 | 172 | // 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. |
MarijnJ | 3:d53674889db3 | 173 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
MarijnJ | 3:d53674889db3 | 174 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
MarijnJ | 3:d53674889db3 | 175 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
MarijnJ | 3:d53674889db3 | 176 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
MarijnJ | 3:d53674889db3 | 177 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
MarijnJ | 3:d53674889db3 | 178 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
MarijnJ | 3:d53674889db3 | 179 | yaw = atan2(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]); |
MarijnJ | 3:d53674889db3 | 180 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
MarijnJ | 3:d53674889db3 | 181 | roll = atan2(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]); |
MarijnJ | 3:d53674889db3 | 182 | pitch *= 180.0f / PI; |
MarijnJ | 3:d53674889db3 | 183 | yaw *= 180.0f / PI; |
MarijnJ | 3:d53674889db3 | 184 | yaw -= 1.05f; // Declination at Delft is 1 degrees 3 minutes on 2018-02-16 |
MarijnJ | 3:d53674889db3 | 185 | roll *= 180.0f / PI; |
MarijnJ | 3:d53674889db3 | 186 | |
MarijnJ | 3:d53674889db3 | 187 | SEGGER_RTT_printf(0, "Yaw: %d Pitch: %d Roll: %d\n\n", toInt(yaw), toInt(pitch), toInt(roll)); |
MarijnJ | 3:d53674889db3 | 188 | //SEGGER_RTT_printf(0, "average rate = %d\n\n\n", toInt((float) sumCount/sum)); |
MarijnJ | 3:d53674889db3 | 189 | |
MarijnJ | 3:d53674889db3 | 190 | count = t.read_ms(); |
MarijnJ | 3:d53674889db3 | 191 | SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------", getTime(count)); |
MarijnJ | 3:d53674889db3 | 192 | |
MarijnJ | 3:d53674889db3 | 193 | if(count > 1<<21) |
MarijnJ | 3:d53674889db3 | 194 | { |
MarijnJ | 3:d53674889db3 | 195 | t.start(); // start the timer over again if ~30 minutes has passed |
MarijnJ | 3:d53674889db3 | 196 | count = 0; |
MarijnJ | 3:d53674889db3 | 197 | deltat= 0; |
MarijnJ | 3:d53674889db3 | 198 | lastUpdate = t.read_us(); |
MarijnJ | 3:d53674889db3 | 199 | shift = (++cycle * 34.9525f); |
MarijnJ | 3:d53674889db3 | 200 | } |
MarijnJ | 3:d53674889db3 | 201 | sum = 0; |
MarijnJ | 3:d53674889db3 | 202 | sumCount = 0; |
MarijnJ | 3:d53674889db3 | 203 | } |
MarijnJ | 3:d53674889db3 | 204 | } |
MarijnJ | 3:d53674889db3 | 205 | } |