Tests Yaw angle
Dependencies: MPU6050IMU mbed
main.cpp
- Committer:
- Reimerguns
- Date:
- 2015-02-20
- Revision:
- 2:8b8ea2b74666
- Parent:
- 0:98f7c7d55051
File content as of revision 2:8b8ea2b74666:
// SDA --- PTE25 // SCL --- PTE24 #include "mbed.h" #include "MPU6050.h" float dt,Yaw,gyroZ=0, lgyroZ = 0; MPU6050 mpu6050; Timer t; Serial pc(USBTX, USBRX); // tx, rx int main(){ pc.baud(9600); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C t.start(); // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); if (whoami == 0x68){ pc.printf("MPU6050 is online..."); wait(1); mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature pc.printf("\nMPU6050 passed self test... Initializing"); wait(2); } else pc.printf("\nDevice did not the pass self-test!\n\r"); } else { pc.printf("Could not connect to MPU6050: \n\r"); pc.printf("%#x \n", whoami); while(1) ; // Loop forever if communication doesn't happen } while(1) { // If data ready bit set, all data registers have new data if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values mpu6050.getGres(); gyroZ = (float)gyroCount[2]*gRes; // - gyroBias[2]; } Now = t.read_us(); dt = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update lastUpdate = Now; Yaw += ((lgyroZ+gyroZ)/2)*dt; lgyroZ = gyroZ; pc.printf("Yaw: %f\n\r", Yaw); } }