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@1:61bf659e4a1f, 2015-06-07 (annotated)

Committer:
xosuuu
Date:
Sun Jun 07 16:48:13 2015 +0000
Revision:
1:61bf659e4a1f
Parent:
0:1a6e8ffa801b
Child:
2:bb20d5091065
Sospecho que el I2C da tantos errores debido a que en el bucle infinito el programa hace continuas llamadas al I2C sin esperar. Por ello se ha querido probar incorporando dentro de bucle infinito waits de 5ms

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