![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Yaw know when you need relative yaw u can use me
Diff: main.cpp
- Revision:
- 0:f463e270d211
diff -r 000000000000 -r f463e270d211 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jul 19 21:12:50 2018 +0000 @@ -0,0 +1,136 @@ +/* MPU9250 Basic Example Code + by: Kris Winer + date: April 1, 2014 + license: Beerware - Use this code however you'd like. If you + find it useful you can buy me a beer some time. + + Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, + getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to + allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and + Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. + + SDA and SCL should have external pull-up resistors (to 3.3V). + 10k resistors are on the EMSENSR-9250 breakout board. + + Hardware setup: + MPU9250 Breakout --------- Arduino + VDD ---------------------- 3.3V + VDDI --------------------- 3.3V + SDA ----------------------- A4 + SCL ----------------------- A5 + GND ---------------------- GND + + Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. + 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. + We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. + We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. + */ + +//#include "ST_F401_84MHZ.h" +//F401_init84 myinit(0); +#include "mbed.h" +#include "MPU9250.h" +//#include "N5110.h" + +// Using NOKIA 5110 monochrome 84 x 48 pixel display +// pin 9 - Serial clock out (SCLK) +// pin 8 - Serial data out (DIN) +// pin 7 - Data/Command select (D/C) +// pin 5 - LCD chip select (CS) +// pin 6 - LCD reset (RST) +//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6); + +float sum = 0; +uint32_t sumCount = 0; +int count = 0; +float yaw = 0; +float drift; +MPU9250 imu(PTE25, PTE24); // SDA, SCL + + Timer t; + + Serial pc(USBTX, USBRX); // tx, rx + + // VCC, SCE, RST, D/C, MOSI,S CLK, LED + // N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7); + + + +int main() +{ + pc.baud(9600); + pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); + + // Read the WHO_AM_I register, this is a good test of communication + uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 + pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); + + if (whoami == 0x71) // WHO_AM_I should always be 0x68 + { + pc.printf("MPU9250 is online...\n\r"); + + wait(1); + + imu.resetMPU9250(); // Reset registers to default in preparation for device calibration + imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + imu.initMPU9250(); + imu.initAK8963(imu.magCalibration); + wait(2); + } + else + { + pc.printf("Could not connect to MPU9250: \n\r"); + pc.printf("%#x \n", whoami); + + while(1) ; // Loop forever if communication doesn't happen + } + + imu.getAres(); // Get accelerometer sensitivity + imu.getGres(); // Get gyro sensitivity + imu.getMres(); // Get magnetometer sensitivity + pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes); + pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes); + pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes); + imu.magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated + imu.magbias[1] = +120.; // User environmental x-axis correction in milliGauss + imu.magbias[2] = +125.; // User environmental x-axis correction in milliGauss + t.start(); + + while(1) { + + // If intPin goes high, all data registers have new data + if(imu.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt + + imu.readAccelData(imu.accelCount); // Read the x/y/z adc values + // Now we'll calculate the accleration value into actual g's + imu.ax = (float)imu.accelCount[0]*imu.aRes - imu.accelBias[0]; // get actual g value, this depends on scale being set + imu.ay = (float)imu.accelCount[1]*imu.aRes - imu.accelBias[1]; + imu.az = (float)imu.accelCount[2]*imu.aRes - imu.accelBias[2]; + + imu.readGyroData(imu.gyroCount); // Read the x/y/z adc values + // Calculate the gyro value into actual degrees per second + imu.gx = (float)imu.gyroCount[0]*imu.gRes - imu.gyroBias[0]; // get actual gyro value, this depends on scale being set + imu.gy = (float)imu.gyroCount[1]*imu.gRes - imu.gyroBias[1]; + imu.gz = (float)imu.gyroCount[2]*imu.gRes - imu.gyroBias[2]; + + imu.readMagData(imu.magCount); // Read the x/y/z adc values + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental corrections + imu.mx = (float)imu.magCount[0]*imu.mRes*imu.magCalibration[0] - imu.magbias[0]; // get actual magnetometer value, this depends on scale being set + imu.my = (float)imu.magCount[1]*imu.mRes*imu.magCalibration[1] - imu.magbias[1]; + imu.mz = (float)imu.magCount[2]*imu.mRes*imu.magCalibration[2] - imu.magbias[2]; + } + + if(imu.gz>.3 || imu.gz < -.3){ + yaw = (yaw - t.read()*imu.gz+drift); + t.reset(); + if(yaw > 360) + yaw -= 360; + if(yaw < 0) + yaw += 360; + pc.printf("Yaw: %f \n\r", yaw); + } + +} +} + \ No newline at end of file