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@2:bb20d5091065, 2015-09-01 (annotated)

Committer:
xosuuu
Date:
Tue Sep 01 14:06:00 2015 +0000
Revision:
2:bb20d5091065
Parent:
1:61bf659e4a1f 
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.

Who changed what in which revision?

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