Farhan Alam / Mbed 2 deprecated MPU9250_withflashmemorytest

Dependencies:   mbed

Fork of MPU9250 by Ilia Manenok

main.cpp@3:c138317c9753, 2017-07-08 (annotated)

Committer:
farhanalam
Date:
Sat Jul 08 14:49:30 2017 +0000
Revision:
3:c138317c9753
Parent:
2:6b427a493d9b
Child:
4:337af8bbd44e
removed floating calculations

Who changed what in which revision?

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