Takujyou_Ishii / Mbed 2 deprecated test_mpu6050

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Madgwickfilter MPU6050

main.cpp@4:fdba5e452d36, 2019-07-10 (annotated)

Committer:
MazeTaka
Date:
Wed Jul 10 10:40:24 2019 +0000
Revision:
4:fdba5e452d36
Parent:
3:9424c6493a75
Child:
5:f41d7b3be417
program for MPU6050 with madgwick filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stream3715 0:07431908151a 1
stream3715 0:07431908151a 2 /* MPU6050 Basic Example Code
stream3715 0:07431908151a 3 by: Kris Winer
stream3715 0:07431908151a 4 date: May 1, 2014
MazeTaka 4:fdba5e452d36 5 license: Beerware - Use this code however you'd like. If you
stream3715 0:07431908151a 6 find it useful you can buy me a beer some time.
MazeTaka 4:fdba5e452d36 7
stream3715 0:07431908151a 8 Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as
MazeTaka 4:fdba5e452d36 9 parameterizing the register addresses. Added display functions to allow display to on breadboard monitor.
stream3715 0:07431908151a 10 No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
MazeTaka 4:fdba5e452d36 11
stream3715 0:07431908151a 12 SDA and SCL should have external pull-up resistors (to 3.3V).
stream3715 0:07431908151a 13 10k resistors worked for me. They should be on the breakout
stream3715 0:07431908151a 14 board.
MazeTaka 4:fdba5e452d36 15
stream3715 0:07431908151a 16 Hardware setup:
stream3715 0:07431908151a 17 MPU6050 Breakout --------- Arduino
stream3715 0:07431908151a 18 3.3V --------------------- 3.3V
stream3715 0:07431908151a 19 SDA ----------------------- A4
stream3715 0:07431908151a 20 SCL ----------------------- A5
stream3715 0:07431908151a 21 GND ---------------------- GND
MazeTaka 4:fdba5e452d36 22
MazeTaka 4:fdba5e452d36 23 Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library.
stream3715 0:07431908151a 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.
stream3715 0:07431908151a 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
stream3715 0:07431908151a 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
stream3715 0:07431908151a 27 */
MazeTaka 4:fdba5e452d36 28
stream3715 0:07431908151a 29 #include "mbed.h"
stream3715 0:07431908151a 30 #include "MPU6050.h"
stream3715 0:07431908151a 31
stream3715 0:07431908151a 32 float sum = 0;
stream3715 0:07431908151a 33 uint32_t sumCount = 0;
stream3715 0:07431908151a 34
MazeTaka 4:fdba5e452d36 35 MPU6050 mpu6050;
MazeTaka 4:fdba5e452d36 36
MazeTaka 4:fdba5e452d36 37 Timer t;
stream3715 3:9424c6493a75 38
MazeTaka 4:fdba5e452d36 39 Serial pc(USBTX, USBRX); // tx, rx
MazeTaka 4:fdba5e452d36 40
MazeTaka 4:fdba5e452d36 41 void gyro_data();
MazeTaka 4:fdba5e452d36 42
MazeTaka 4:fdba5e452d36 43 Ticker gyro_tick;
MazeTaka 4:fdba5e452d36 44
stream3715 0:07431908151a 45 int main()
stream3715 0:07431908151a 46 {
MazeTaka 4:fdba5e452d36 47 pc.baud(9600);
MazeTaka 4:fdba5e452d36 48
MazeTaka 4:fdba5e452d36 49 //Set up I2C
MazeTaka 4:fdba5e452d36 50 i2c.frequency(400000); // use fast (400 kHz) I2C
MazeTaka 4:fdba5e452d36 51
MazeTaka 4:fdba5e452d36 52 t.start();
MazeTaka 4:fdba5e452d36 53
MazeTaka 4:fdba5e452d36 54 // Read the WHO_AM_I register, this is a good test of communication
MazeTaka 4:fdba5e452d36 55 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
MazeTaka 4:fdba5e452d36 56 pc.printf("I AM 0x%x\n\r", whoami);
MazeTaka 4:fdba5e452d36 57 pc.printf("I SHOULD BE 0x68\n\r");
MazeTaka 4:fdba5e452d36 58
MazeTaka 4:fdba5e452d36 59 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
MazeTaka 4:fdba5e452d36 60 pc.printf("MPU6050 is online...");
MazeTaka 4:fdba5e452d36 61 wait(1);
stream3715 3:9424c6493a75 62
MazeTaka 4:fdba5e452d36 63 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
MazeTaka 4:fdba5e452d36 64 pc.printf("x-axis self test: acceleration trim within : ");
MazeTaka 4:fdba5e452d36 65 pc.printf("%f", SelfTest[0]);
MazeTaka 4:fdba5e452d36 66 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 67 pc.printf("y-axis self test: acceleration trim within : ");
MazeTaka 4:fdba5e452d36 68 pc.printf("%f", SelfTest[1]);
MazeTaka 4:fdba5e452d36 69 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 70 pc.printf("z-axis self test: acceleration trim within : ");
MazeTaka 4:fdba5e452d36 71 pc.printf("%f", SelfTest[2]);
MazeTaka 4:fdba5e452d36 72 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 73 pc.printf("x-axis self test: gyration trim within : ");
MazeTaka 4:fdba5e452d36 74 pc.printf("%f", SelfTest[3]);
MazeTaka 4:fdba5e452d36 75 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 76 pc.printf("y-axis self test: gyration trim within : ");
MazeTaka 4:fdba5e452d36 77 pc.printf("%f", SelfTest[4]);
MazeTaka 4:fdba5e452d36 78 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 79 pc.printf("z-axis self test: gyration trim within : ");
MazeTaka 4:fdba5e452d36 80 pc.printf("%f", SelfTest[5]);
MazeTaka 4:fdba5e452d36 81 pc.printf("% of factory value \n\r");
MazeTaka 4:fdba5e452d36 82 wait(1);
stream3715 0:07431908151a 83
MazeTaka 4:fdba5e452d36 84 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) {
MazeTaka 4:fdba5e452d36 85 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
MazeTaka 4:fdba5e452d36 86 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
MazeTaka 4:fdba5e452d36 87 mpu6050.initMPU6050();
MazeTaka 4:fdba5e452d36 88 pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
stream3715 0:07431908151a 89
MazeTaka 4:fdba5e452d36 90 wait(2);
MazeTaka 4:fdba5e452d36 91 } else {
MazeTaka 4:fdba5e452d36 92 pc.printf("Device did not the pass self-test!\n\r");
MazeTaka 4:fdba5e452d36 93
MazeTaka 4:fdba5e452d36 94 }
MazeTaka 4:fdba5e452d36 95 } else {
MazeTaka 4:fdba5e452d36 96 pc.printf("Could not connect to MPU6050: \n\r");
MazeTaka 4:fdba5e452d36 97 pc.printf("%#x \n", whoami);
MazeTaka 4:fdba5e452d36 98
MazeTaka 4:fdba5e452d36 99 while(1) ; // Loop forever if communication doesn't happen
stream3715 0:07431908151a 100 }
MazeTaka 4:fdba5e452d36 101 gyro_tick.attach_us(&gyro_data,50000);
stream3715 0:07431908151a 102
stream3715 0:07431908151a 103
stream3715 0:07431908151a 104
MazeTaka 4:fdba5e452d36 105 while(1) {
MazeTaka 4:fdba5e452d36 106 pc.printf("a{x,y,z}={%7.3f,%7.3f,%7.3f},T=%7.3f,g{x,y,z}={%7.3f,%7.3f,%7.3f},",ax,ay,az,temperature,gx,gy,gz);
MazeTaka 4:fdba5e452d36 107 pc.printf("Yaw, Pitch, Roll: %9.4f %9.4f %9.4f", yaw, pitch, roll);
MazeTaka 4:fdba5e452d36 108 pc.printf("beta=%5.3f,zeta=%5.3f,",beta,zeta);
MazeTaka 4:fdba5e452d36 109 //pc.printf("ax=%5d,ay=%5d,az=%5d,temp=%5d,gx=%5d,gy=%5d,gz=%5d,",accelCount[0],accelCount[1],accelCount[2],tempCount,gyroCount[0],gyroCount[1],gyroCount[2]);
MazeTaka 4:fdba5e452d36 110 pc.printf("\r\n");
MazeTaka 4:fdba5e452d36 111 }
MazeTaka 4:fdba5e452d36 112
MazeTaka 4:fdba5e452d36 113 }
MazeTaka 4:fdba5e452d36 114
MazeTaka 4:fdba5e452d36 115 void gyro_data()
MazeTaka 4:fdba5e452d36 116 {
MazeTaka 4:fdba5e452d36 117
MazeTaka 4:fdba5e452d36 118 // If data ready bit set, all data registers have new data
MazeTaka 4:fdba5e452d36 119 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
MazeTaka 4:fdba5e452d36 120 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
MazeTaka 4:fdba5e452d36 121 mpu6050.getAres();
stream3715 0:07431908151a 122
MazeTaka 4:fdba5e452d36 123 // Now we'll calculate the accleration value into actual g's
MazeTaka 4:fdba5e452d36 124 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
MazeTaka 4:fdba5e452d36 125 ay = (float)accelCount[1]*aRes - accelBias[1];
MazeTaka 4:fdba5e452d36 126 az = (float)accelCount[2]*aRes - accelBias[2];
MazeTaka 4:fdba5e452d36 127
MazeTaka 4:fdba5e452d36 128 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
MazeTaka 4:fdba5e452d36 129 mpu6050.getGres();
MazeTaka 4:fdba5e452d36 130
MazeTaka 4:fdba5e452d36 131 // Calculate the gyro value into actual degrees per second
MazeTaka 4:fdba5e452d36 132 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
MazeTaka 4:fdba5e452d36 133 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
MazeTaka 4:fdba5e452d36 134 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
MazeTaka 4:fdba5e452d36 135
MazeTaka 4:fdba5e452d36 136 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
MazeTaka 4:fdba5e452d36 137 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
MazeTaka 4:fdba5e452d36 138 }
MazeTaka 4:fdba5e452d36 139
stream3715 0:07431908151a 140 Now = t.read_us();
stream3715 0:07431908151a 141 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
stream3715 0:07431908151a 142 lastUpdate = Now;
MazeTaka 4:fdba5e452d36 143
stream3715 0:07431908151a 144 sum += deltat;
stream3715 0:07431908151a 145 sumCount++;
MazeTaka 4:fdba5e452d36 146
stream3715 0:07431908151a 147 if(lastUpdate - firstUpdate > 10000000.0f) {
MazeTaka 4:fdba5e452d36 148 beta = 0.04; // decrease filter gain after stabilized
MazeTaka 4:fdba5e452d36 149 zeta = 0.015; // increasey bias drift gain after stabilized
stream3715 0:07431908151a 150 }
MazeTaka 4:fdba5e452d36 151
MazeTaka 4:fdba5e452d36 152 // Pass gyro rate as rad/s
stream3715 0:07431908151a 153 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
stream3715 0:07431908151a 154
stream3715 0:07431908151a 155 // Serial print and/or display at 0.5 s rate independent of data rates
stream3715 0:07431908151a 156 delt_t = t.read_ms() - count;
MazeTaka 4:fdba5e452d36 157 if (delt_t > 1) { // update LCD once per half-second independent of read rate*/
stream3715 0:07431908151a 158
stream3715 0:07431908151a 159
MazeTaka 4:fdba5e452d36 160
MazeTaka 4:fdba5e452d36 161
MazeTaka 4:fdba5e452d36 162 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
MazeTaka 4:fdba5e452d36 163 // In this coordinate system, the positive z-axis is down toward Earth.
MazeTaka 4:fdba5e452d36 164 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
MazeTaka 4:fdba5e452d36 165 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
MazeTaka 4:fdba5e452d36 166 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
MazeTaka 4:fdba5e452d36 167 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
MazeTaka 4:fdba5e452d36 168 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
MazeTaka 4:fdba5e452d36 169 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
MazeTaka 4:fdba5e452d36 170 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
MazeTaka 4:fdba5e452d36 171 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
MazeTaka 4:fdba5e452d36 172 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
MazeTaka 4:fdba5e452d36 173 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
MazeTaka 4:fdba5e452d36 174 pitch *= 180.0f / PI;
MazeTaka 4:fdba5e452d36 175 yaw *= 180.0f / PI;
MazeTaka 4:fdba5e452d36 176 roll *= 180.0f / PI;
stream3715 0:07431908151a 177
stream3715 0:07431908151a 178 // pc.printf("Yaw, Pitch, Roll: \n\r");
stream3715 0:07431908151a 179 // pc.printf("%f", yaw);
stream3715 0:07431908151a 180 // pc.printf(", ");
stream3715 0:07431908151a 181 // pc.printf("%f", pitch);
stream3715 0:07431908151a 182 // pc.printf(", ");
stream3715 0:07431908151a 183 // pc.printf("%f\n\r", roll);
stream3715 0:07431908151a 184 // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
stream3715 0:07431908151a 185
MazeTaka 4:fdba5e452d36 186 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
MazeTaka 4:fdba5e452d36 187 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
MazeTaka 4:fdba5e452d36 188
MazeTaka 4:fdba5e452d36 189 myled= !myled;
MazeTaka 4:fdba5e452d36 190 count = t.read_ms();
MazeTaka 4:fdba5e452d36 191 sum = 0;
MazeTaka 4:fdba5e452d36 192 sumCount = 0;
MazeTaka 4:fdba5e452d36 193 }
MazeTaka 4:fdba5e452d36 194 }