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.
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
Generated on Fri Jul 15 2022 19:11:53 by
1.7.2