Iván Arias / MPU9150_Test

MPU9150AHRS adapted to a lpc1768 uC displaying data via serial port to pc.

Dependencies:   MPU9150AHRS

main.cpp@0:648796e4886d, 2019-04-08 (annotated)

Committer:
ivaariasga
Date:
Mon Apr 08 20:56:48 2019 +0000
Revision:
0:648796e4886d
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivaariasga 0:648796e4886d 1 /* MPU9150 Basic Example Code
ivaariasga 0:648796e4886d 2 by: Kris Winer
ivaariasga 0:648796e4886d 3 date: April 1, 2014
ivaariasga 0:648796e4886d 4 license: Beerware - Use this code however you'd like. If you
ivaariasga 0:648796e4886d 5 find it useful you can buy me a beer some time.
ivaariasga 0:648796e4886d 6
ivaariasga 0:648796e4886d 7 Demonstrate basic MPU-9150 functionality including parameterizing the register addresses, initializing the sensor,
ivaariasga 0:648796e4886d 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
ivaariasga 0:648796e4886d 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
ivaariasga 0:648796e4886d 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
ivaariasga 0:648796e4886d 11
ivaariasga 0:648796e4886d 12 SDA and SCL should have external pull-up resistors (to 3.3V).
ivaariasga 0:648796e4886d 13 10k resistors are on the EMSENSR-9250 breakout board.
ivaariasga 0:648796e4886d 14
ivaariasga 0:648796e4886d 15 Hardware setup:
ivaariasga 0:648796e4886d 16 MPU9150 Breakout --------- Arduino
ivaariasga 0:648796e4886d 17 VDD ---------------------- 3.3V
ivaariasga 0:648796e4886d 18 VDDI --------------------- 3.3V
ivaariasga 0:648796e4886d 19 SDA ----------------------- A4
ivaariasga 0:648796e4886d 20 SCL ----------------------- A5
ivaariasga 0:648796e4886d 21 GND ---------------------- GND
ivaariasga 0:648796e4886d 22
ivaariasga 0:648796e4886d 23 Note: The MPU9150 is an I2C sensor and uses the Arduino Wire library.
ivaariasga 0:648796e4886d 24 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
ivaariasga 0:648796e4886d 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
ivaariasga 0:648796e4886d 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
ivaariasga 0:648796e4886d 27 */
ivaariasga 0:648796e4886d 28
ivaariasga 0:648796e4886d 29
ivaariasga 0:648796e4886d 30 #include "mbed.h"
ivaariasga 0:648796e4886d 31 #include "MPU9150.h"
ivaariasga 0:648796e4886d 32
ivaariasga 0:648796e4886d 33 float sum = 0;
ivaariasga 0:648796e4886d 34 uint32_t sumCount = 0, mcount = 0;
ivaariasga 0:648796e4886d 35 char buffer[14];
ivaariasga 0:648796e4886d 36
ivaariasga 0:648796e4886d 37 MPU9150 MPU9150;
ivaariasga 0:648796e4886d 38
ivaariasga 0:648796e4886d 39 Timer t;
ivaariasga 0:648796e4886d 40
ivaariasga 0:648796e4886d 41 Serial pc(USBTX, USBRX); // tx, rx
ivaariasga 0:648796e4886d 42
ivaariasga 0:648796e4886d 43
ivaariasga 0:648796e4886d 44
ivaariasga 0:648796e4886d 45 int main()
ivaariasga 0:648796e4886d 46 {
ivaariasga 0:648796e4886d 47 pc.baud(9600);
ivaariasga 0:648796e4886d 48
ivaariasga 0:648796e4886d 49 //Set up I2C
ivaariasga 0:648796e4886d 50 i2c.frequency(400000); // use fast (400 kHz) I2C
ivaariasga 0:648796e4886d 51
ivaariasga 0:648796e4886d 52 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
ivaariasga 0:648796e4886d 53
ivaariasga 0:648796e4886d 54 t.start();
ivaariasga 0:648796e4886d 55
ivaariasga 0:648796e4886d 56 // Read the WHO_AM_I register, this is a good test of communication
ivaariasga 0:648796e4886d 57 uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150); // Read WHO_AM_I register for MPU-9250
ivaariasga 0:648796e4886d 58 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
ivaariasga 0:648796e4886d 59
ivaariasga 0:648796e4886d 60 if (whoami == 0x68) // WHO_AM_I should be 0x68
ivaariasga 0:648796e4886d 61 {
ivaariasga 0:648796e4886d 62 pc.printf("MPU9150 WHO_AM_I is 0x%x\n\r", whoami);
ivaariasga 0:648796e4886d 63 pc.printf("MPU9150 is online...\n\r");
ivaariasga 0:648796e4886d 64 sprintf(buffer, "0x%x", whoami);
ivaariasga 0:648796e4886d 65 wait(1);
ivaariasga 0:648796e4886d 66
ivaariasga 0:648796e4886d 67 MPU9150.MPU9150SelfTest(SelfTest);
ivaariasga 0:648796e4886d 68 pc.printf("x-axis self test: acceleration trim within %f of factory value\n\r", SelfTest[0]);
ivaariasga 0:648796e4886d 69 pc.printf("y-axis self test: acceleration trim within %f of factory value\n\r", SelfTest[1]);
ivaariasga 0:648796e4886d 70 pc.printf("z-axis self test: acceleration trim within %f of factory value\n\r", SelfTest[2]);
ivaariasga 0:648796e4886d 71 pc.printf("x-axis self test: gyration trim within %f of factory value\n\r", SelfTest[3]);
ivaariasga 0:648796e4886d 72 pc.printf("y-axis self test: gyration trim within %f of factory value\n\r", SelfTest[4]);
ivaariasga 0:648796e4886d 73 pc.printf("z-axis self test: gyration trim within %f of factory value\n\r", SelfTest[5]);
ivaariasga 0:648796e4886d 74 wait(1);
ivaariasga 0:648796e4886d 75 MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
ivaariasga 0:648796e4886d 76 MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ivaariasga 0:648796e4886d 77 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
ivaariasga 0:648796e4886d 78 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
ivaariasga 0:648796e4886d 79 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
ivaariasga 0:648796e4886d 80 pc.printf("x accel bias = %f\n\r", accelBias[0]);
ivaariasga 0:648796e4886d 81 pc.printf("y accel bias = %f\n\r", accelBias[1]);
ivaariasga 0:648796e4886d 82 pc.printf("z accel bias = %f\n\r", accelBias[2]);
ivaariasga 0:648796e4886d 83 wait(1);
ivaariasga 0:648796e4886d 84 MPU9150.initMPU9150();
ivaariasga 0:648796e4886d 85 pc.printf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
ivaariasga 0:648796e4886d 86 MPU9150.initAK8975A(magCalibration);
ivaariasga 0:648796e4886d 87 pc.printf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
ivaariasga 0:648796e4886d 88 }
ivaariasga 0:648796e4886d 89 else
ivaariasga 0:648796e4886d 90 {
ivaariasga 0:648796e4886d 91 pc.printf("Could not connect to MPU9150: \n\r");
ivaariasga 0:648796e4886d 92 pc.printf("%#x \n", whoami);
ivaariasga 0:648796e4886d 93
ivaariasga 0:648796e4886d 94 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
ivaariasga 0:648796e4886d 95
ivaariasga 0:648796e4886d 96 while(1) ; // Loop forever if communication doesn't happen
ivaariasga 0:648796e4886d 97 }
ivaariasga 0:648796e4886d 98
ivaariasga 0:648796e4886d 99 uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
ivaariasga 0:648796e4886d 100 MPU9150.getAres(); // Get accelerometer sensitivity
ivaariasga 0:648796e4886d 101 MPU9150.getGres(); // Get gyro sensitivity
ivaariasga 0:648796e4886d 102 mRes = 10.*1229./4096.; // Conversion from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
ivaariasga 0:648796e4886d 103 // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
ivaariasga 0:648796e4886d 104 // like the gyro and accelerometer biases
ivaariasga 0:648796e4886d 105 magbias[0] = -5.; // User environmental x-axis correction in milliGauss
ivaariasga 0:648796e4886d 106 magbias[1] = -95.; // User environmental y-axis correction in milliGauss
ivaariasga 0:648796e4886d 107 magbias[2] = -260.; // User environmental z-axis correction in milliGauss
ivaariasga 0:648796e4886d 108
ivaariasga 0:648796e4886d 109
ivaariasga 0:648796e4886d 110 while(1) {
ivaariasga 0:648796e4886d 111
ivaariasga 0:648796e4886d 112 // If intPin goes high, all data registers have new data
ivaariasga 0:648796e4886d 113 if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
ivaariasga 0:648796e4886d 114
ivaariasga 0:648796e4886d 115 MPU9150.readAccelData(accelCount); // Read the x/y/z adc values
ivaariasga 0:648796e4886d 116 // Now we'll calculate the accleration value into actual g's
ivaariasga 0:648796e4886d 117 ax = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set
ivaariasga 0:648796e4886d 118 ay = (float)accelCount[1]*aRes; // - accelBias[1];
ivaariasga 0:648796e4886d 119 az = (float)accelCount[2]*aRes; // - accelBias[2];
ivaariasga 0:648796e4886d 120
ivaariasga 0:648796e4886d 121 MPU9150.readGyroData(gyroCount); // Read the x/y/z adc values
ivaariasga 0:648796e4886d 122 // Calculate the gyro value into actual degrees per second
ivaariasga 0:648796e4886d 123 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
ivaariasga 0:648796e4886d 124 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
ivaariasga 0:648796e4886d 125 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
ivaariasga 0:648796e4886d 126
ivaariasga 0:648796e4886d 127 mcount++;
ivaariasga 0:648796e4886d 128 if (mcount > 200/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)
ivaariasga 0:648796e4886d 129 MPU9150.readMagData(magCount); // Read the x/y/z adc values
ivaariasga 0:648796e4886d 130 // Calculate the magnetometer values in milliGauss
ivaariasga 0:648796e4886d 131 // Include factory calibration per data sheet and user environmental corrections
ivaariasga 0:648796e4886d 132 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
ivaariasga 0:648796e4886d 133 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
ivaariasga 0:648796e4886d 134 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
ivaariasga 0:648796e4886d 135 mcount = 0;
ivaariasga 0:648796e4886d 136 }
ivaariasga 0:648796e4886d 137 }
ivaariasga 0:648796e4886d 138
ivaariasga 0:648796e4886d 139 Now = t.read_us();
ivaariasga 0:648796e4886d 140 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ivaariasga 0:648796e4886d 141 lastUpdate = Now;
ivaariasga 0:648796e4886d 142
ivaariasga 0:648796e4886d 143 sum += deltat;
ivaariasga 0:648796e4886d 144 sumCount++;
ivaariasga 0:648796e4886d 145
ivaariasga 0:648796e4886d 146 // if(lastUpdate - firstUpdate > 10000000.0f) {
ivaariasga 0:648796e4886d 147 // beta = 0.04; // decrease filter gain after stabilized
ivaariasga 0:648796e4886d 148 // zeta = 0.015; // increasey bias drift gain after stabilized
ivaariasga 0:648796e4886d 149 // }
ivaariasga 0:648796e4886d 150
ivaariasga 0:648796e4886d 151 // Pass gyro rate as rad/s
ivaariasga 0:648796e4886d 152 // MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ivaariasga 0:648796e4886d 153 MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
ivaariasga 0:648796e4886d 154
ivaariasga 0:648796e4886d 155 // Serial print and/or display at 0.5 s rate independent of data rates
ivaariasga 0:648796e4886d 156 delt_t = t.read_ms() - cont;
ivaariasga 0:648796e4886d 157 if (delt_t > 500) { // update LCD once per half-second independent of read rate
ivaariasga 0:648796e4886d 158
ivaariasga 0:648796e4886d 159 pc.printf("ax = %f", 1000*ax);
ivaariasga 0:648796e4886d 160 pc.printf(" ay = %f", 1000*ay);
ivaariasga 0:648796e4886d 161 pc.printf(" az = %f mg\n\r", 1000*az);
ivaariasga 0:648796e4886d 162
ivaariasga 0:648796e4886d 163 pc.printf("gx = %f", gx);
ivaariasga 0:648796e4886d 164 pc.printf(" gy = %f", gy);
ivaariasga 0:648796e4886d 165 pc.printf(" gz = %f deg/s\n\r", gz);
ivaariasga 0:648796e4886d 166
ivaariasga 0:648796e4886d 167 pc.printf("gx = %f", mx);
ivaariasga 0:648796e4886d 168 pc.printf(" gy = %f", my);
ivaariasga 0:648796e4886d 169 pc.printf(" gz = %f mG\n\r", mz);
ivaariasga 0:648796e4886d 170
ivaariasga 0:648796e4886d 171 tempCount = MPU9150.readTempData(); // Read the adc values
ivaariasga 0:648796e4886d 172 temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
ivaariasga 0:648796e4886d 173 pc.printf(" temperature = %f C\n\r", temperature);
ivaariasga 0:648796e4886d 174
ivaariasga 0:648796e4886d 175 pc.printf("q0 = %f\n\r", q[0]);
ivaariasga 0:648796e4886d 176 pc.printf("q1 = %f\n\r", q[1]);
ivaariasga 0:648796e4886d 177 pc.printf("q2 = %f\n\r", q[2]);
ivaariasga 0:648796e4886d 178 pc.printf("q3 = %f\n\r", q[3]);
ivaariasga 0:648796e4886d 179
ivaariasga 0:648796e4886d 180 /* lcd.clear();
ivaariasga 0:648796e4886d 181 lcd.printString("MPU9150", 0, 0);
ivaariasga 0:648796e4886d 182 lcd.printString("x y z", 0, 1);
ivaariasga 0:648796e4886d 183 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
ivaariasga 0:648796e4886d 184 lcd.printString(buffer, 0, 2);
ivaariasga 0:648796e4886d 185 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
ivaariasga 0:648796e4886d 186 lcd.printString(buffer, 0, 3);
ivaariasga 0:648796e4886d 187 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
ivaariasga 0:648796e4886d 188 lcd.printString(buffer, 0, 4);
ivaariasga 0:648796e4886d 189 */
ivaariasga 0:648796e4886d 190 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
ivaariasga 0:648796e4886d 191 // In this coordinate system, the positive z-axis is down toward Earth.
ivaariasga 0:648796e4886d 192 // 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.
ivaariasga 0:648796e4886d 193 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
ivaariasga 0:648796e4886d 194 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
ivaariasga 0:648796e4886d 195 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
ivaariasga 0:648796e4886d 196 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
ivaariasga 0:648796e4886d 197 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
ivaariasga 0:648796e4886d 198 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
ivaariasga 0:648796e4886d 199 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]);
ivaariasga 0:648796e4886d 200 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ivaariasga 0:648796e4886d 201 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]);
ivaariasga 0:648796e4886d 202 pitch *= 180.0f / PI;
ivaariasga 0:648796e4886d 203 yaw *= 180.0f / PI;
ivaariasga 0:648796e4886d 204 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
ivaariasga 0:648796e4886d 205 roll *= 180.0f / PI;
ivaariasga 0:648796e4886d 206
ivaariasga 0:648796e4886d 207 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
ivaariasga 0:648796e4886d 208 pc.printf("average rate = %f\n\r", (float) sumCount/sum);
ivaariasga 0:648796e4886d 209 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
ivaariasga 0:648796e4886d 210 // lcd.printString(buffer, 0, 4);
ivaariasga 0:648796e4886d 211 // sprintf(buffer, "rate = %f", (float) sumCount/sum);
ivaariasga 0:648796e4886d 212 // lcd.printString(buffer, 0, 5);
ivaariasga 0:648796e4886d 213
ivaariasga 0:648796e4886d 214 myled= !myled;
ivaariasga 0:648796e4886d 215 cont = t.read_ms();
ivaariasga 0:648796e4886d 216
ivaariasga 0:648796e4886d 217 if(cont > 1<<21) {
ivaariasga 0:648796e4886d 218 t.start(); // start the timer over again if ~30 minutes has passed
ivaariasga 0:648796e4886d 219 cont = 0;
ivaariasga 0:648796e4886d 220 deltat= 0;
ivaariasga 0:648796e4886d 221 lastUpdate = t.read_us();
ivaariasga 0:648796e4886d 222 }
ivaariasga 0:648796e4886d 223 sum = 0;
ivaariasga 0:648796e4886d 224 sumCount = 0;
ivaariasga 0:648796e4886d 225 }
ivaariasga 0:648796e4886d 226 }
ivaariasga 0:648796e4886d 227
ivaariasga 0:648796e4886d 228 }