Josué Olmeda / Mbed 2 deprecated STM32L152withMPU-9250

This program use the STM32L152 developement board and the MPU-9250 9-axis InvenSense movement sensor. The communication between both devices is made through I2C serial interface. This algorithm calibrates and reads data from accelerometer, gyroscope, magnetometer and the internal temperature sensor. The lecture is made each time has a new meassured value (both gyro and accel data). A comunication with a computer is made using serial interface. The user can see the data measured with 1 second update rate.

Dependencies:   mbed

Dependents:   2_MPU9250_attitude

main.cpp@0:1a6e8ffa801b, 2015-04-30 (annotated)

Committer:
xosuuu
Date:
Thu Apr 30 11:22:00 2015 +0000
Revision:
0:1a6e8ffa801b
Child:
1:61bf659e4a1f
void getCompassOrientation()

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xosuuu 0:1a6e8ffa801b 1 #include "mbed.h"
xosuuu 0:1a6e8ffa801b 2 #include "MPU9250.h"
xosuuu 0:1a6e8ffa801b 3
xosuuu 0:1a6e8ffa801b 4 //-----------------------------------------------
xosuuu 0:1a6e8ffa801b 5 // Hyperterminal configuration:
xosuuu 0:1a6e8ffa801b 6 // 9600 bauds, 8-bit data, 1 stop bit, no parity
xosuuu 0:1a6e8ffa801b 7 //-----------------------------------------------
xosuuu 0:1a6e8ffa801b 8
xosuuu 0:1a6e8ffa801b 9 Serial pc(USBTX, USBRX); // Default: 9600 bauds, 8-bit data, 1 stop bit, no parity
xosuuu 0:1a6e8ffa801b 10 MPU9250 mpu9250;
xosuuu 0:1a6e8ffa801b 11 Timer t;
xosuuu 0:1a6e8ffa801b 12 //DigitalOut myled(LED1);
xosuuu 0:1a6e8ffa801b 13
xosuuu 0:1a6e8ffa801b 14 float sum = 0;
xosuuu 0:1a6e8ffa801b 15 uint32_t sumCount = 0;
xosuuu 0:1a6e8ffa801b 16 char buffer[14];
xosuuu 0:1a6e8ffa801b 17 uint8_t dato_leido[2];
xosuuu 0:1a6e8ffa801b 18
xosuuu 0:1a6e8ffa801b 19 int main() {
xosuuu 0:1a6e8ffa801b 20
xosuuu 0:1a6e8ffa801b 21 //___ Set up I2C: use fast (400 kHz) I2C ___
xosuuu 0:1a6e8ffa801b 22 i2c.frequency(400000);
xosuuu 0:1a6e8ffa801b 23
xosuuu 0:1a6e8ffa801b 24 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
xosuuu 0:1a6e8ffa801b 25
xosuuu 0:1a6e8ffa801b 26 t.start(); // Timer ON
xosuuu 0:1a6e8ffa801b 27
xosuuu 0:1a6e8ffa801b 28 // Read the WHO_AM_I register, this is a good test of communication
xosuuu 0:1a6e8ffa801b 29 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
xosuuu 0:1a6e8ffa801b 30
xosuuu 0:1a6e8ffa801b 31 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
xosuuu 0:1a6e8ffa801b 32 if (I2Cstate != 0) // error on I2C
xosuuu 0:1a6e8ffa801b 33 pc.printf("I2C failure while reading WHO_AM_I register");
xosuuu 0:1a6e8ffa801b 34
xosuuu 0:1a6e8ffa801b 35 if (whoami == 0x71) // WHO_AM_I should always be 0x71
xosuuu 0:1a6e8ffa801b 36 {
xosuuu 0:1a6e8ffa801b 37 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
xosuuu 0:1a6e8ffa801b 38 pc.printf("MPU9250 is online...\n\r");
xosuuu 0:1a6e8ffa801b 39 sprintf(buffer, "0x%x", whoami);
xosuuu 0:1a6e8ffa801b 40 wait(1);
xosuuu 0:1a6e8ffa801b 41
xosuuu 0:1a6e8ffa801b 42 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
xosuuu 0:1a6e8ffa801b 43
xosuuu 0:1a6e8ffa801b 44 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
xosuuu 0:1a6e8ffa801b 45 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
xosuuu 0:1a6e8ffa801b 46 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
xosuuu 0:1a6e8ffa801b 47 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
xosuuu 0:1a6e8ffa801b 48 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
xosuuu 0:1a6e8ffa801b 49 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
xosuuu 0:1a6e8ffa801b 50 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
xosuuu 0:1a6e8ffa801b 51
xosuuu 0:1a6e8ffa801b 52 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
xosuuu 0:1a6e8ffa801b 53 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
xosuuu 0:1a6e8ffa801b 54 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
xosuuu 0:1a6e8ffa801b 55 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
xosuuu 0:1a6e8ffa801b 56 pc.printf("x accel bias = %f\n\r", accelBias[0]);
xosuuu 0:1a6e8ffa801b 57 pc.printf("y accel bias = %f\n\r", accelBias[1]);
xosuuu 0:1a6e8ffa801b 58 pc.printf("z accel bias = %f\n\r", accelBias[2]);
xosuuu 0:1a6e8ffa801b 59 wait(2);
xosuuu 0:1a6e8ffa801b 60
xosuuu 0:1a6e8ffa801b 61 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
xosuuu 0:1a6e8ffa801b 62 mpu9250.initMPU9250();
xosuuu 0:1a6e8ffa801b 63 pc.printf("MPU9250 initialized for active data mode....\n\r");
xosuuu 0:1a6e8ffa801b 64
xosuuu 0:1a6e8ffa801b 65 // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
xosuuu 0:1a6e8ffa801b 66 mpu9250.initAK8963(magCalibration);
xosuuu 0:1a6e8ffa801b 67 pc.printf("AK8963 initialized for active data mode....\n\r");
xosuuu 0:1a6e8ffa801b 68 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
xosuuu 0:1a6e8ffa801b 69 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
xosuuu 0:1a6e8ffa801b 70 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
xosuuu 0:1a6e8ffa801b 71 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
xosuuu 0:1a6e8ffa801b 72 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
xosuuu 0:1a6e8ffa801b 73 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
xosuuu 0:1a6e8ffa801b 74 wait(1);
xosuuu 0:1a6e8ffa801b 75 }
xosuuu 0:1a6e8ffa801b 76
xosuuu 0:1a6e8ffa801b 77 else // Connection failure
xosuuu 0:1a6e8ffa801b 78 {
xosuuu 0:1a6e8ffa801b 79 pc.printf("Could not connect to MPU9250: \n\r");
xosuuu 0:1a6e8ffa801b 80 pc.printf("%#x \n", whoami);
xosuuu 0:1a6e8ffa801b 81 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
xosuuu 0:1a6e8ffa801b 82 while(1) ; // Loop forever if communication doesn't happen
xosuuu 0:1a6e8ffa801b 83 }
xosuuu 0:1a6e8ffa801b 84
xosuuu 0:1a6e8ffa801b 85 mpu9250.getAres(); // Get accelerometer sensitivity
xosuuu 0:1a6e8ffa801b 86 mpu9250.getGres(); // Get gyro sensitivity
xosuuu 0:1a6e8ffa801b 87 mpu9250.getMres(); // Get magnetometer sensitivity
xosuuu 0:1a6e8ffa801b 88 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
xosuuu 0:1a6e8ffa801b 89 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
xosuuu 0:1a6e8ffa801b 90 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
xosuuu 0:1a6e8ffa801b 91 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
xosuuu 0:1a6e8ffa801b 92 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
xosuuu 0:1a6e8ffa801b 93 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
xosuuu 0:1a6e8ffa801b 94
xosuuu 0:1a6e8ffa801b 95 while(1) {
xosuuu 0:1a6e8ffa801b 96
xosuuu 0:1a6e8ffa801b 97 // If intPin goes high, all data registers have new data
xosuuu 0:1a6e8ffa801b 98 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
xosuuu 0:1a6e8ffa801b 99
xosuuu 0:1a6e8ffa801b 100 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
xosuuu 0:1a6e8ffa801b 101 // Now we'll calculate the accleration value into actual g's
xosuuu 0:1a6e8ffa801b 102 if (I2Cstate != 0) //error on I2C
xosuuu 0:1a6e8ffa801b 103 pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
xosuuu 0:1a6e8ffa801b 104 else{ // I2C read or write ok
xosuuu 0:1a6e8ffa801b 105 I2Cstate = 1;
xosuuu 0:1a6e8ffa801b 106 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
xosuuu 0:1a6e8ffa801b 107 ay = (float)accelCount[1]*aRes - accelBias[1];
xosuuu 0:1a6e8ffa801b 108 az = (float)accelCount[2]*aRes - accelBias[2];
xosuuu 0:1a6e8ffa801b 109 }
xosuuu 0:1a6e8ffa801b 110
xosuuu 0:1a6e8ffa801b 111 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
xosuuu 0:1a6e8ffa801b 112 // Calculate the gyro value into actual degrees per second
xosuuu 0:1a6e8ffa801b 113 if (I2Cstate != 0) //error on I2C
xosuuu 0:1a6e8ffa801b 114 pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
xosuuu 0:1a6e8ffa801b 115 else{ // I2C read or write ok
xosuuu 0:1a6e8ffa801b 116 I2Cstate = 1;
xosuuu 0:1a6e8ffa801b 117 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
xosuuu 0:1a6e8ffa801b 118 gy = (float)gyroCount[1]*gRes - gyroBias[1];
xosuuu 0:1a6e8ffa801b 119 gz = (float)gyroCount[2]*gRes - gyroBias[2];
xosuuu 0:1a6e8ffa801b 120 }
xosuuu 0:1a6e8ffa801b 121
xosuuu 0:1a6e8ffa801b 122 mpu9250.readMagData(magCount); // Read the x/y/z adc values
xosuuu 0:1a6e8ffa801b 123 // Calculate the magnetometer values in milliGauss
xosuuu 0:1a6e8ffa801b 124 // Include factory calibration per data sheet and user environmental corrections
xosuuu 0:1a6e8ffa801b 125 if (I2Cstate != 0) //error on I2C
xosuuu 0:1a6e8ffa801b 126 pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
xosuuu 0:1a6e8ffa801b 127 else{ // I2C read or write ok
xosuuu 0:1a6e8ffa801b 128 I2Cstate = 1;
xosuuu 0:1a6e8ffa801b 129 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
xosuuu 0:1a6e8ffa801b 130 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
xosuuu 0:1a6e8ffa801b 131 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
xosuuu 0:1a6e8ffa801b 132 }
xosuuu 0:1a6e8ffa801b 133 }
xosuuu 0:1a6e8ffa801b 134
xosuuu 0:1a6e8ffa801b 135 Now = t.read_us();
xosuuu 0:1a6e8ffa801b 136 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
xosuuu 0:1a6e8ffa801b 137 lastUpdate = Now;
xosuuu 0:1a6e8ffa801b 138 sum += deltat;
xosuuu 0:1a6e8ffa801b 139 sumCount++;
xosuuu 0:1a6e8ffa801b 140
xosuuu 0:1a6e8ffa801b 141 // Pass gyro rate as rad/s
xosuuu 0:1a6e8ffa801b 142 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
xosuuu 0:1a6e8ffa801b 143 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
xosuuu 0:1a6e8ffa801b 144
xosuuu 0:1a6e8ffa801b 145
xosuuu 0:1a6e8ffa801b 146 // Serial print and/or display at 1.5 s rate independent of data rates
xosuuu 0:1a6e8ffa801b 147 delt_t = t.read_ms() - count;
xosuuu 0:1a6e8ffa801b 148 if (delt_t > 1500) { // update LCD once per half-second independent of read rate
xosuuu 0:1a6e8ffa801b 149 pc.printf("ax = %f", 1000*ax);
xosuuu 0:1a6e8ffa801b 150 pc.printf(" ay = %f", 1000*ay);
xosuuu 0:1a6e8ffa801b 151 pc.printf(" az = %f mg\n\r", 1000*az);
xosuuu 0:1a6e8ffa801b 152 pc.printf("gx = %f", gx);
xosuuu 0:1a6e8ffa801b 153 pc.printf(" gy = %f", gy);
xosuuu 0:1a6e8ffa801b 154 pc.printf(" gz = %f deg/s\n\r", gz);
xosuuu 0:1a6e8ffa801b 155 pc.printf("mx = %f", mx);
xosuuu 0:1a6e8ffa801b 156 pc.printf(" my = %f", my);
xosuuu 0:1a6e8ffa801b 157 pc.printf(" mz = %f mG\n\r", mz);
xosuuu 0:1a6e8ffa801b 158
xosuuu 0:1a6e8ffa801b 159
xosuuu 0:1a6e8ffa801b 160 tempCount = mpu9250.readTempData(); // Read the adc values
xosuuu 0:1a6e8ffa801b 161 if (I2Cstate != 0) //error on I2C
xosuuu 0:1a6e8ffa801b 162 pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
xosuuu 0:1a6e8ffa801b 163 else{ // I2C read or write ok
xosuuu 0:1a6e8ffa801b 164 I2Cstate = 1;
xosuuu 0:1a6e8ffa801b 165 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
xosuuu 0:1a6e8ffa801b 166 pc.printf(" temperature = %f C\n\r", temperature);
xosuuu 0:1a6e8ffa801b 167 }
xosuuu 0:1a6e8ffa801b 168 pc.printf("q0 = %f\n\r", q[0]);
xosuuu 0:1a6e8ffa801b 169 pc.printf("q1 = %f\n\r", q[1]);
xosuuu 0:1a6e8ffa801b 170 pc.printf("q2 = %f\n\r", q[2]);
xosuuu 0:1a6e8ffa801b 171 pc.printf("q3 = %f\n\r", q[3]);
xosuuu 0:1a6e8ffa801b 172
xosuuu 0:1a6e8ffa801b 173
xosuuu 0:1a6e8ffa801b 174
xosuuu 0:1a6e8ffa801b 175
xosuuu 0:1a6e8ffa801b 176 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
xosuuu 0:1a6e8ffa801b 177 // In this coordinate system, the positive z-axis is down toward Earth.
xosuuu 0:1a6e8ffa801b 178 // 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.
xosuuu 0:1a6e8ffa801b 179 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
xosuuu 0:1a6e8ffa801b 180 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
xosuuu 0:1a6e8ffa801b 181 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
xosuuu 0:1a6e8ffa801b 182 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
xosuuu 0:1a6e8ffa801b 183 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
xosuuu 0:1a6e8ffa801b 184 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
xosuuu 0:1a6e8ffa801b 185
xosuuu 0:1a6e8ffa801b 186 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]);
xosuuu 0:1a6e8ffa801b 187 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
xosuuu 0:1a6e8ffa801b 188 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]);
xosuuu 0:1a6e8ffa801b 189 pitch *= 180.0f / PI;
xosuuu 0:1a6e8ffa801b 190 yaw *= 180.0f / PI;
xosuuu 0:1a6e8ffa801b 191 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
xosuuu 0:1a6e8ffa801b 192 roll *= 180.0f / PI;
xosuuu 0:1a6e8ffa801b 193
xosuuu 0:1a6e8ffa801b 194 /*
xosuuu 0:1a6e8ffa801b 195 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
xosuuu 0:1a6e8ffa801b 196 pc.printf("average rate = %f\n\r", (float) sumCount/sum);
xosuuu 0:1a6e8ffa801b 197 */
xosuuu 0:1a6e8ffa801b 198
xosuuu 0:1a6e8ffa801b 199
xosuuu 0:1a6e8ffa801b 200 myled= !myled;
xosuuu 0:1a6e8ffa801b 201 count = t.read_ms();
xosuuu 0:1a6e8ffa801b 202
xosuuu 0:1a6e8ffa801b 203 if(count > 1<<21) {
xosuuu 0:1a6e8ffa801b 204 t.start(); // start the timer over again if ~30 minutes has passed
xosuuu 0:1a6e8ffa801b 205 count = 0;
xosuuu 0:1a6e8ffa801b 206 deltat= 0;
xosuuu 0:1a6e8ffa801b 207 lastUpdate = t.read_us();
xosuuu 0:1a6e8ffa801b 208 }
xosuuu 0:1a6e8ffa801b 209 sum = 0;
xosuuu 0:1a6e8ffa801b 210 sumCount = 0;
xosuuu 0:1a6e8ffa801b 211 }
xosuuu 0:1a6e8ffa801b 212 }
xosuuu 0:1a6e8ffa801b 213 }