Chengwei Zhang / Mbed 2 deprecated MPU6050-DMP_test_JJ

demo of the optimized MPU6050 library

Dependencies:   MPU6050-DMP mbed

Fork of MPU6050-DMP_test by Match Lab

main.cpp@3:0fd6f597d8dd, 2015-02-26 (annotated)

Committer:
zcw607
Date:
Thu Feb 26 04:02:36 2015 +0000
Revision:
3:0fd6f597d8dd
Parent:
2:7ef42101b204 
test modified library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Match314 0:a89e198f9bf7 1 /* Demo code for MPU6050 DMP
Match314 0:a89e198f9bf7 2 * I thank Ian Hua.
Match314 0:a89e198f9bf7 3 * Copyright (c) 2015 Match
Match314 0:a89e198f9bf7 4 *
Match314 1:ea5ef4879bc4 5 * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Match314 0:a89e198f9bf7 6 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Match314 0:a89e198f9bf7 7 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Match314 0:a89e198f9bf7 8 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Match314 0:a89e198f9bf7 9 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Match314 1:ea5ef4879bc4 10 * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN
Match314 1:ea5ef4879bc4 11 * THE PROGRAM.
Match314 0:a89e198f9bf7 12 */
Match314 0:a89e198f9bf7 13
Match314 0:a89e198f9bf7 14
Match314 0:a89e198f9bf7 15 // Define Necessary.
Match314 0:a89e198f9bf7 16 //#define OUTPUT_QUATERNION
Match314 0:a89e198f9bf7 17 //#define OUTPUT_EULER
Match314 0:a89e198f9bf7 18 #define OUTPUT_ROLL_PITCH_YAW
Match314 0:a89e198f9bf7 19 //#define OUTPUT_FOR_TEAPOT
Match314 0:a89e198f9bf7 20 //#define OUTPUT_TEMPERATURE
Match314 0:a89e198f9bf7 21
Match314 0:a89e198f9bf7 22
Match314 0:a89e198f9bf7 23 #include "MPU6050_6Axis_MotionApps20.h"
Match314 0:a89e198f9bf7 24 #include "mbed.h"
Match314 0:a89e198f9bf7 25 #include "config.h"
Match314 0:a89e198f9bf7 26 #include <stdio.h>
Match314 0:a89e198f9bf7 27
Match314 0:a89e198f9bf7 28
Match314 0:a89e198f9bf7 29 #define DEG_TO_RAD(x) ( x * 0.01745329 )
Match314 0:a89e198f9bf7 30 #define RAD_TO_DEG(x) ( x * 57.29578 )
Match314 0:a89e198f9bf7 31
Match314 0:a89e198f9bf7 32
Match314 0:a89e198f9bf7 33 RawSerial pc(USBTX, USBRX);
zcw607 3:0fd6f597d8dd 34 MPU6050 mpu(P0_20, P0_22); // sda, scl pin
zcw607 3:0fd6f597d8dd 35 InterruptIn INT0(P0_21); // INT0 pin
Match314 0:a89e198f9bf7 36
Match314 0:a89e198f9bf7 37 const int FIFO_BUFFER_SIZE = 128;
Match314 0:a89e198f9bf7 38 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
Match314 0:a89e198f9bf7 39 uint16_t fifoCount;
Match314 0:a89e198f9bf7 40 uint16_t packetSize;
Match314 0:a89e198f9bf7 41 bool dmpReady;
Match314 0:a89e198f9bf7 42 uint8_t mpuIntStatus;
Match314 0:a89e198f9bf7 43 const int snprintf_buffer_size = 100;
Match314 0:a89e198f9bf7 44 char snprintf_buffer[snprintf_buffer_size];
Match314 0:a89e198f9bf7 45 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
Match314 0:a89e198f9bf7 46
Match314 0:a89e198f9bf7 47 struct Offset {
Match314 0:a89e198f9bf7 48 int16_t ax, ay, az;
Match314 0:a89e198f9bf7 49 int16_t gx, gy, gz;
Match314 0:a89e198f9bf7 50 }offset = {150, -350, 1000, -110, 5, 0}; // Measured values
Match314 0:a89e198f9bf7 51
Match314 0:a89e198f9bf7 52 struct MPU6050_DmpData {
Match314 0:a89e198f9bf7 53 Quaternion q;
Match314 0:a89e198f9bf7 54 VectorFloat gravity; // g
Match314 0:a89e198f9bf7 55 float roll, pitch, yaw; // rad
Match314 0:a89e198f9bf7 56 }dmpData;
Match314 0:a89e198f9bf7 57
Match314 0:a89e198f9bf7 58 bool Init();
Match314 0:a89e198f9bf7 59 void dmpDataUpdate();
Match314 0:a89e198f9bf7 60
Match314 0:a89e198f9bf7 61
Match314 0:a89e198f9bf7 62 int main() {
Match314 0:a89e198f9bf7 63 MBED_ASSERT(Init() == true);
Match314 0:a89e198f9bf7 64
Match314 0:a89e198f9bf7 65 while(1) {
Match314 0:a89e198f9bf7 66
Match314 0:a89e198f9bf7 67 }
Match314 0:a89e198f9bf7 68 }
Match314 0:a89e198f9bf7 69
Match314 0:a89e198f9bf7 70
Match314 0:a89e198f9bf7 71 bool Init() {
Match314 0:a89e198f9bf7 72 pc.baud(PC_BAUDRATE);
Match314 0:a89e198f9bf7 73
Match314 0:a89e198f9bf7 74 INT0.mode(PullDown);
Match314 0:a89e198f9bf7 75 INT0.fall(dmpDataUpdate);
Match314 0:a89e198f9bf7 76
zcw607 3:0fd6f597d8dd 77 //mpu.initialize();
zcw607 3:0fd6f597d8dd 78
zcw607 3:0fd6f597d8dd 79 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
zcw607 3:0fd6f597d8dd 80 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
zcw607 3:0fd6f597d8dd 81 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
zcw607 3:0fd6f597d8dd 82 mpu.setDLPFMode(MPU6050_DLPF_BW_20); //10,20,42,98,188
zcw607 3:0fd6f597d8dd 83 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
zcw607 3:0fd6f597d8dd 84 mpu.setSleepEnabled(false);
zcw607 3:0fd6f597d8dd 85
Match314 0:a89e198f9bf7 86 if (mpu.testConnection()) {
Match314 0:a89e198f9bf7 87 pc.puts("MPU6050 test connection passed.\n");
Match314 0:a89e198f9bf7 88 } else {
Match314 0:a89e198f9bf7 89 pc.puts("MPU6050 test connection failed.\n");
Match314 0:a89e198f9bf7 90 return false;
Match314 0:a89e198f9bf7 91 }
Match314 0:a89e198f9bf7 92 if (mpu.dmpInitialize() == 0) {
Match314 0:a89e198f9bf7 93 pc.puts("succeed in MPU6050 DMP Initializing.\n");
Match314 0:a89e198f9bf7 94 } else {
Match314 0:a89e198f9bf7 95 pc.puts("failed in MPU6050 DMP Initializing.\n");
Match314 0:a89e198f9bf7 96 return false;
Match314 0:a89e198f9bf7 97 }
Match314 0:a89e198f9bf7 98 mpu.setXAccelOffset(offset.ax);
Match314 0:a89e198f9bf7 99 mpu.setYAccelOffset(offset.ay);
Match314 0:a89e198f9bf7 100 mpu.setZAccelOffset(offset.az);
Match314 0:a89e198f9bf7 101 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
Match314 0:a89e198f9bf7 102 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
Match314 0:a89e198f9bf7 103 mpu.setXGyroOffsetUser(offset.gx);
Match314 0:a89e198f9bf7 104 mpu.setYGyroOffsetUser(offset.gy);
Match314 0:a89e198f9bf7 105 mpu.setZGyroOffsetUser(offset.gz);
Match314 0:a89e198f9bf7 106 mpu.setDMPEnabled(true); // Enable DMP
Match314 0:a89e198f9bf7 107 packetSize = mpu.dmpGetFIFOPacketSize();
Match314 0:a89e198f9bf7 108 dmpReady = true; // Enable interrupt.
Match314 0:a89e198f9bf7 109
Match314 0:a89e198f9bf7 110 pc.puts("Init finish!\n");
Match314 0:a89e198f9bf7 111
Match314 0:a89e198f9bf7 112 return true;
Match314 0:a89e198f9bf7 113 }
Match314 0:a89e198f9bf7 114
Match314 0:a89e198f9bf7 115
Match314 0:a89e198f9bf7 116 void dmpDataUpdate() {
Match314 0:a89e198f9bf7 117 // Check that this interrupt has enabled.
Match314 0:a89e198f9bf7 118 if (dmpReady == false) return;
Match314 0:a89e198f9bf7 119
Match314 0:a89e198f9bf7 120 mpuIntStatus = mpu.getIntStatus();
Match314 0:a89e198f9bf7 121 fifoCount = mpu.getFIFOCount();
Match314 0:a89e198f9bf7 122
Match314 0:a89e198f9bf7 123 // Check that this interrupt is a FIFO buffer overflow interrupt.
Match314 0:a89e198f9bf7 124 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
Match314 0:a89e198f9bf7 125 mpu.resetFIFO();
Match314 0:a89e198f9bf7 126 pc.puts("FIFO overflow!\n");
Match314 0:a89e198f9bf7 127 return;
Match314 0:a89e198f9bf7 128
Match314 0:a89e198f9bf7 129 // Check that this interrupt is a Data Ready interrupt.
Match314 0:a89e198f9bf7 130 } else if (mpuIntStatus & 0x02) {
Match314 0:a89e198f9bf7 131 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
Match314 0:a89e198f9bf7 132
Match314 0:a89e198f9bf7 133 mpu.getFIFOBytes(fifoBuffer, packetSize);
Match314 0:a89e198f9bf7 134
Match314 0:a89e198f9bf7 135 #ifdef OUTPUT_QUATERNION
Match314 0:a89e198f9bf7 136 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
Match314 0:a89e198f9bf7 137 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Quaternion : w=%f, x=%f, y=%f, z=%f\n", dmpData.q.w, dmpData.q.x, dmpData.q.y, dmpData.q.z ) < 0 ) return;
Match314 0:a89e198f9bf7 138 pc.puts(snprintf_buffer);
Match314 0:a89e198f9bf7 139 #endif
Match314 0:a89e198f9bf7 140
Match314 0:a89e198f9bf7 141 #ifdef OUTPUT_EULER
Match314 0:a89e198f9bf7 142 float euler[3];
Match314 0:a89e198f9bf7 143 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
Match314 0:a89e198f9bf7 144 mpu.dmpGetEuler(euler, &dmpData.q);
Match314 0:a89e198f9bf7 145 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Euler : psi=%fdeg, theta=%fdeg, phi=%fdeg\n", RAD_TO_DEG(euler[0]), RAD_TO_DEG(euler[1]), RAD_TO_DEG(euler[2]) ) < 0 ) return;
Match314 0:a89e198f9bf7 146 pc.puts(snprintf_buffer);
Match314 0:a89e198f9bf7 147 #endif
Match314 0:a89e198f9bf7 148
Match314 0:a89e198f9bf7 149 #ifdef OUTPUT_ROLL_PITCH_YAW
Match314 0:a89e198f9bf7 150 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
Match314 0:a89e198f9bf7 151 mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
Match314 0:a89e198f9bf7 152 float rollPitchYaw[3];
Match314 0:a89e198f9bf7 153 mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
Match314 0:a89e198f9bf7 154 dmpData.roll = rollPitchYaw[2];
Match314 0:a89e198f9bf7 155 dmpData.pitch = rollPitchYaw[1];
Match314 0:a89e198f9bf7 156 dmpData.yaw = rollPitchYaw[0];
Match314 0:a89e198f9bf7 157
Match314 0:a89e198f9bf7 158 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return;
Match314 0:a89e198f9bf7 159 pc.puts(snprintf_buffer);
Match314 0:a89e198f9bf7 160 #endif
Match314 0:a89e198f9bf7 161
Match314 0:a89e198f9bf7 162 #ifdef OUTPUT_FOR_TEAPOT
Match314 0:a89e198f9bf7 163 teapotPacket[2] = fifoBuffer[0];
Match314 0:a89e198f9bf7 164 teapotPacket[3] = fifoBuffer[1];
Match314 0:a89e198f9bf7 165 teapotPacket[4] = fifoBuffer[4];
Match314 0:a89e198f9bf7 166 teapotPacket[5] = fifoBuffer[5];
Match314 0:a89e198f9bf7 167 teapotPacket[6] = fifoBuffer[8];
Match314 0:a89e198f9bf7 168 teapotPacket[7] = fifoBuffer[9];
Match314 0:a89e198f9bf7 169 teapotPacket[8] = fifoBuffer[12];
Match314 0:a89e198f9bf7 170 teapotPacket[9] = fifoBuffer[13];
Match314 0:a89e198f9bf7 171 for (uint8_t i = 0; i < 14; i++) {
Match314 0:a89e198f9bf7 172 pc.putc(teapotPacket[i]);
Match314 0:a89e198f9bf7 173 }
Match314 0:a89e198f9bf7 174 #endif
Match314 0:a89e198f9bf7 175
Match314 0:a89e198f9bf7 176 #ifdef OUTPUT_TEMPERATURE
Match314 0:a89e198f9bf7 177 float temp = mpu.getTemperature() / 340.0 + 36.53;
Match314 0:a89e198f9bf7 178 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
Match314 0:a89e198f9bf7 179 pc.puts(snprintf_buffer);
Match314 0:a89e198f9bf7 180 #endif
Match314 0:a89e198f9bf7 181
Match314 0:a89e198f9bf7 182 pc.puts("\n");
Match314 0:a89e198f9bf7 183 }
Match314 0:a89e198f9bf7 184 }