anyThing Connected Team / Mbed 2 deprecated MPU9250_simple

Sensor data verwerking

Dependencies:   SEGGER_RTT mbed

Fork of MPU9250AHRS by Kris Winer

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?

UserRevisionLine numberNew 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 }