Jesus Fausto / Mbed 2 deprecated IMU_MPU925X_YAW

Yaw know when you need relative yaw u can use me

Dependencies:   MPU9250 mbed

main.cpp@0:f463e270d211, 2018-07-19 (annotated)

Committer:
jvfausto
Date:
Thu Jul 19 21:12:50 2018 +0000
Revision:
0:f463e270d211
Hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jvfausto 0:f463e270d211 1 /* MPU9250 Basic Example Code
jvfausto 0:f463e270d211 2 by: Kris Winer
jvfausto 0:f463e270d211 3 date: April 1, 2014
jvfausto 0:f463e270d211 4 license: Beerware - Use this code however you'd like. If you
jvfausto 0:f463e270d211 5 find it useful you can buy me a beer some time.
jvfausto 0:f463e270d211 6
jvfausto 0:f463e270d211 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
jvfausto 0:f463e270d211 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
jvfausto 0:f463e270d211 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
jvfausto 0:f463e270d211 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
jvfausto 0:f463e270d211 11
jvfausto 0:f463e270d211 12 SDA and SCL should have external pull-up resistors (to 3.3V).
jvfausto 0:f463e270d211 13 10k resistors are on the EMSENSR-9250 breakout board.
jvfausto 0:f463e270d211 14
jvfausto 0:f463e270d211 15 Hardware setup:
jvfausto 0:f463e270d211 16 MPU9250 Breakout --------- Arduino
jvfausto 0:f463e270d211 17 VDD ---------------------- 3.3V
jvfausto 0:f463e270d211 18 VDDI --------------------- 3.3V
jvfausto 0:f463e270d211 19 SDA ----------------------- A4
jvfausto 0:f463e270d211 20 SCL ----------------------- A5
jvfausto 0:f463e270d211 21 GND ---------------------- GND
jvfausto 0:f463e270d211 22
jvfausto 0:f463e270d211 23 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
jvfausto 0:f463e270d211 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.
jvfausto 0:f463e270d211 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
jvfausto 0:f463e270d211 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
jvfausto 0:f463e270d211 27 */
jvfausto 0:f463e270d211 28
jvfausto 0:f463e270d211 29 //#include "ST_F401_84MHZ.h"
jvfausto 0:f463e270d211 30 //F401_init84 myinit(0);
jvfausto 0:f463e270d211 31 #include "mbed.h"
jvfausto 0:f463e270d211 32 #include "MPU9250.h"
jvfausto 0:f463e270d211 33 //#include "N5110.h"
jvfausto 0:f463e270d211 34
jvfausto 0:f463e270d211 35 // Using NOKIA 5110 monochrome 84 x 48 pixel display
jvfausto 0:f463e270d211 36 // pin 9 - Serial clock out (SCLK)
jvfausto 0:f463e270d211 37 // pin 8 - Serial data out (DIN)
jvfausto 0:f463e270d211 38 // pin 7 - Data/Command select (D/C)
jvfausto 0:f463e270d211 39 // pin 5 - LCD chip select (CS)
jvfausto 0:f463e270d211 40 // pin 6 - LCD reset (RST)
jvfausto 0:f463e270d211 41 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
jvfausto 0:f463e270d211 42
jvfausto 0:f463e270d211 43 float sum = 0;
jvfausto 0:f463e270d211 44 uint32_t sumCount = 0;
jvfausto 0:f463e270d211 45 int count = 0;
jvfausto 0:f463e270d211 46 float yaw = 0;
jvfausto 0:f463e270d211 47 float drift;
jvfausto 0:f463e270d211 48 MPU9250 imu(PTE25, PTE24); // SDA, SCL
jvfausto 0:f463e270d211 49
jvfausto 0:f463e270d211 50 Timer t;
jvfausto 0:f463e270d211 51
jvfausto 0:f463e270d211 52 Serial pc(USBTX, USBRX); // tx, rx
jvfausto 0:f463e270d211 53
jvfausto 0:f463e270d211 54 // VCC, SCE, RST, D/C, MOSI,S CLK, LED
jvfausto 0:f463e270d211 55 // N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
jvfausto 0:f463e270d211 56
jvfausto 0:f463e270d211 57
jvfausto 0:f463e270d211 58
jvfausto 0:f463e270d211 59 int main()
jvfausto 0:f463e270d211 60 {
jvfausto 0:f463e270d211 61 pc.baud(9600);
jvfausto 0:f463e270d211 62 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
jvfausto 0:f463e270d211 63
jvfausto 0:f463e270d211 64 // Read the WHO_AM_I register, this is a good test of communication
jvfausto 0:f463e270d211 65 uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
jvfausto 0:f463e270d211 66 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
jvfausto 0:f463e270d211 67
jvfausto 0:f463e270d211 68 if (whoami == 0x71) // WHO_AM_I should always be 0x68
jvfausto 0:f463e270d211 69 {
jvfausto 0:f463e270d211 70 pc.printf("MPU9250 is online...\n\r");
jvfausto 0:f463e270d211 71
jvfausto 0:f463e270d211 72 wait(1);
jvfausto 0:f463e270d211 73
jvfausto 0:f463e270d211 74 imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
jvfausto 0:f463e270d211 75 imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
jvfausto 0:f463e270d211 76 imu.initMPU9250();
jvfausto 0:f463e270d211 77 imu.initAK8963(imu.magCalibration);
jvfausto 0:f463e270d211 78 wait(2);
jvfausto 0:f463e270d211 79 }
jvfausto 0:f463e270d211 80 else
jvfausto 0:f463e270d211 81 {
jvfausto 0:f463e270d211 82 pc.printf("Could not connect to MPU9250: \n\r");
jvfausto 0:f463e270d211 83 pc.printf("%#x \n", whoami);
jvfausto 0:f463e270d211 84
jvfausto 0:f463e270d211 85 while(1) ; // Loop forever if communication doesn't happen
jvfausto 0:f463e270d211 86 }
jvfausto 0:f463e270d211 87
jvfausto 0:f463e270d211 88 imu.getAres(); // Get accelerometer sensitivity
jvfausto 0:f463e270d211 89 imu.getGres(); // Get gyro sensitivity
jvfausto 0:f463e270d211 90 imu.getMres(); // Get magnetometer sensitivity
jvfausto 0:f463e270d211 91 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
jvfausto 0:f463e270d211 92 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
jvfausto 0:f463e270d211 93 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
jvfausto 0:f463e270d211 94 imu.magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
jvfausto 0:f463e270d211 95 imu.magbias[1] = +120.; // User environmental x-axis correction in milliGauss
jvfausto 0:f463e270d211 96 imu.magbias[2] = +125.; // User environmental x-axis correction in milliGauss
jvfausto 0:f463e270d211 97 t.start();
jvfausto 0:f463e270d211 98
jvfausto 0:f463e270d211 99 while(1) {
jvfausto 0:f463e270d211 100
jvfausto 0:f463e270d211 101 // If intPin goes high, all data registers have new data
jvfausto 0:f463e270d211 102 if(imu.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
jvfausto 0:f463e270d211 103
jvfausto 0:f463e270d211 104 imu.readAccelData(imu.accelCount); // Read the x/y/z adc values
jvfausto 0:f463e270d211 105 // Now we'll calculate the accleration value into actual g's
jvfausto 0:f463e270d211 106 imu.ax = (float)imu.accelCount[0]*imu.aRes - imu.accelBias[0]; // get actual g value, this depends on scale being set
jvfausto 0:f463e270d211 107 imu.ay = (float)imu.accelCount[1]*imu.aRes - imu.accelBias[1];
jvfausto 0:f463e270d211 108 imu.az = (float)imu.accelCount[2]*imu.aRes - imu.accelBias[2];
jvfausto 0:f463e270d211 109
jvfausto 0:f463e270d211 110 imu.readGyroData(imu.gyroCount); // Read the x/y/z adc values
jvfausto 0:f463e270d211 111 // Calculate the gyro value into actual degrees per second
jvfausto 0:f463e270d211 112 imu.gx = (float)imu.gyroCount[0]*imu.gRes - imu.gyroBias[0]; // get actual gyro value, this depends on scale being set
jvfausto 0:f463e270d211 113 imu.gy = (float)imu.gyroCount[1]*imu.gRes - imu.gyroBias[1];
jvfausto 0:f463e270d211 114 imu.gz = (float)imu.gyroCount[2]*imu.gRes - imu.gyroBias[2];
jvfausto 0:f463e270d211 115
jvfausto 0:f463e270d211 116 imu.readMagData(imu.magCount); // Read the x/y/z adc values
jvfausto 0:f463e270d211 117 // Calculate the magnetometer values in milliGauss
jvfausto 0:f463e270d211 118 // Include factory calibration per data sheet and user environmental corrections
jvfausto 0:f463e270d211 119 imu.mx = (float)imu.magCount[0]*imu.mRes*imu.magCalibration[0] - imu.magbias[0]; // get actual magnetometer value, this depends on scale being set
jvfausto 0:f463e270d211 120 imu.my = (float)imu.magCount[1]*imu.mRes*imu.magCalibration[1] - imu.magbias[1];
jvfausto 0:f463e270d211 121 imu.mz = (float)imu.magCount[2]*imu.mRes*imu.magCalibration[2] - imu.magbias[2];
jvfausto 0:f463e270d211 122 }
jvfausto 0:f463e270d211 123
jvfausto 0:f463e270d211 124 if(imu.gz>.3 || imu.gz < -.3){
jvfausto 0:f463e270d211 125 yaw = (yaw - t.read()*imu.gz+drift);
jvfausto 0:f463e270d211 126 t.reset();
jvfausto 0:f463e270d211 127 if(yaw > 360)
jvfausto 0:f463e270d211 128 yaw -= 360;
jvfausto 0:f463e270d211 129 if(yaw < 0)
jvfausto 0:f463e270d211 130 yaw += 360;
jvfausto 0:f463e270d211 131 pc.printf("Yaw: %f \n\r", yaw);
jvfausto 0:f463e270d211 132 }
jvfausto 0:f463e270d211 133
jvfausto 0:f463e270d211 134 }
jvfausto 0:f463e270d211 135 }
jvfausto 0:f463e270d211 136