Read accelerometer and gyroscope measurements from the MPU-6050. The MPU-60X0 is the world’s first integrated 6-axis MotionTracking device that combines a 3-axis gyroscope, 3-axis accelerometer, and a Digital Motion Processor™ (DMP) all in a small 4x4x0.9mm package. With its dedicated I2C sensor bus, it directly accepts inputs from an external 3-axis compass to provide a complete 9-axis MotionFusion™ output.

Dependencies:   MPU6050

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /*#################################################################################
00002 
00003 Program Name    : MPU6050 Hello World 
00004 Author          : Crispin Mukalay
00005 Date Modified   : 17/10/2018
00006 Compiler        : ARMmbed
00007 Tested On       : NUCLEO-F446RE
00008 
00009 Description     : Demonstrates the use of the MPU6050 gryroscope/accelerometer/temperature
00010                   sensor to read gyroscope 3-axis angular velocities(°/s) and accelerometer
00011                   3-axis accelerations (°).
00012                   
00013 Requirements    : * NUCLEO-F446RE Board
00014                   * MPU6050 Module
00015               
00016 Circuit         : * The MPU6050 module is connected as follows:
00017                     VCC                      -   3.3V
00018                     GND                      -   GND
00019                     SCL                      -   PB10 (I2C2_SCL pin)
00020                     SDA                      -   PB3 (I2C2_SDA pin)
00021 
00022 ####################################################################################*/
00023 
00024 #include "mbed.h"
00025 #include "MPU6050.h"
00026 #include <math.h>
00027 #define pi 3.141592654
00028 
00029 MPU6050 AccGyro(PB_3, PB_10); // Create an MPU object called AccGyro
00030 
00031 Serial pc(SERIAL_TX, SERIAL_RX);    //To use the PC as a console (display output)
00032 
00033 int16_t Ax, Ay, Az, Gx, Gy, Gz;
00034 float Ax_f, Ay_f, Az_f;
00035 double Gx_f, Gy_f, Gz_f;
00036 float Ax_f_sum, Ay_f_sum, Az_f_sum, Gx_f_sum, Gy_f_sum, Gz_f_sum;
00037 float roll, pitch, yaw;
00038 
00039 int main() {
00040     
00041     uint16_t AccelReadings[3] = {0, 0, 0};
00042     uint16_t GyroReadings[3] = {0, 0, 0};
00043     uint8_t DevId;
00044 
00045     pc.printf("Starting MPU6050 test...\n");
00046     DevId = AccGyro.getWhoAmI();
00047     
00048     if(DevId == 0x68){
00049         pc.printf("\n");
00050         pc.printf("MPU6050 detected...\n");
00051         pc.printf("Device ID is: 0x%02x\n", DevId);
00052         pc.printf("\n");
00053     }else{
00054         pc.printf("\n");
00055         pc.printf("MPU6050 not found...\n");
00056         while(1);
00057     }
00058     
00059     // The device will come up in sleep mode upon power-up.
00060     AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC);    // Disable sleep mode
00061     wait(.001);
00062     
00063     // Full scale, +/-2000°/s, 16.4LSB°/s.
00064     AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off.
00065     wait(.001);
00066     
00067     // Full scale, +/-16g, 2048LSB/g.
00068     AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g);    // Gyroscope self-test trigger off.
00069     wait(.001);
00070         
00071     while (true) {
00072         
00073        wait(0.2);
00074        
00075        Ax_f_sum = 0;
00076        Ay_f_sum = 0;
00077        Az_f_sum = 0;
00078        Gx_f_sum = 0;
00079        Gy_f_sum = 0;
00080        Gz_f_sum = 0;
00081        
00082        for(int i = 0; i < 10; i = i + 1)    // Take ten analog input readings
00083        {
00084             AccGyro.readAccel(AccelReadings);   // Extract accelerometer measurements
00085             AccGyro.readGyro(GyroReadings);     // Extract gyroscope measurements
00086             
00087             // 2s complement acclerometer and gyroscope values 
00088             Ax = AccelReadings[0];
00089             Ay = AccelReadings[1];
00090             Az = AccelReadings[2]; 
00091             Gx = GyroReadings[0];
00092             Gy = GyroReadings[1];
00093             Gz = GyroReadings[2];
00094                         
00095             // Add every reading to the sum variables     
00096             Ax_f_sum = Ax_f_sum + (float)Ax;
00097             Ay_f_sum = Ay_f_sum + (float)Ay;
00098             Az_f_sum = Az_f_sum + (float)Az;
00099             Gx_f_sum = Gx_f_sum + (float)Gx;
00100             Gy_f_sum = Gy_f_sum + (float)Gy;
00101             Gz_f_sum = Gz_f_sum + (float)Gz;
00102         }
00103         
00104         // Divide by 10 to get the averaged value
00105         Ax_f = Ax_f_sum / 10;
00106         Ay_f = Ay_f_sum / 10;
00107         Az_f = Az_f_sum / 10;
00108         Gx_f = Gx_f_sum / 10;
00109         Gy_f = Gy_f_sum / 10;
00110         Gz_f = Gz_f_sum / 10;
00111          
00112         // 1. Calculate actual roll, pitch and yaw angles in degrees
00113         // 2. Calibrate readings by adding or substracting the off-set
00114         roll = (180/pi)*(atan(Ax_f/(sqrt((Ay_f*Ay_f)+(Az_f*Az_f))))) - 4.36;
00115         pitch = (180/pi)*(atan(Ay_f/(sqrt((Ax_f*Ax_f)+(Az_f*Az_f))))) - 0.063;
00116         yaw = (180/pi)*(atan((sqrt((Ax_f*Ax_f)+(Ay_f*Ay_f)))/Az_f)) - 3.93;
00117         
00118         // Convert gyroscope readings into degrees/s
00119         Gx_f = Gx_f / 131.0;
00120         Gy_f = Gy_f / 131.0;
00121         Gz_f = Gz_f / 131.0;
00122                 
00123         pc.printf("Gyro(deg/s) X: %.3f Y: %.3f Z: %.3f || Accel(deg) Roll: %.3f, Pitch: %.3f, Yaw: %.3f \n", Gx_f, Gy_f, Gz_f, roll, pitch, yaw);
00124         
00125     }
00126 }
00127