main.cpp

00001 /* Demo code for MPU6050 DMP
00002  * I thank Ian Hua.
00003  * Copyright (c) 2015 Match
00004  *
00005  * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00006  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00007  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00008  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00009  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00010  * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN
00011  * THE PROGRAM.
00012  */
00013 
00014 
00015 // Define Necessary.
00016 //#define OUTPUT_QUATERNION
00017 //#define OUTPUT_EULER
00018 #define OUTPUT_ROLL_PITCH_YAW
00019 //#define OUTPUT_FOR_TEAPOT
00020 //#define OUTPUT_TEMPERATURE
00021 
00022 
00023 #include "MPU6050_6Axis_MotionApps20.h"
00024 #include "mbed.h"
00025 #include "config.h"
00026 #include <stdio.h>
00027 
00028 
00029 #define DEG_TO_RAD(x) ( x * 0.01745329 )
00030 #define RAD_TO_DEG(x) ( x * 57.29578 )
00031 
00032 
00033 RawSerial pc(USBTX, USBRX);
00034 MPU6050 mpu(dp5, dp27);     // sda, scl pin
00035 InterruptIn INT0(dp13);     // INT0 pin
00036 
00037 const int FIFO_BUFFER_SIZE = 128;
00038 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
00039 uint16_t fifoCount;
00040 uint16_t packetSize;
00041 bool dmpReady;
00042 uint8_t mpuIntStatus;
00043 const int snprintf_buffer_size = 100;
00044 char snprintf_buffer[snprintf_buffer_size];
00045 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00046 
00047 struct Offset {
00048     int16_t ax, ay, az;
00049     int16_t gx, gy, gz;
00050 }offset = {150, -350, 1000, -110, 5, 0};    // Measured values
00051 
00052 struct MPU6050_DmpData {
00053     Quaternion q;
00054     VectorFloat gravity;    // g
00055     float roll, pitch, yaw;     // rad
00056 }dmpData;
00057 
00058 bool Init();
00059 void dmpDataUpdate();
00060 
00061 
00062 int main() {
00063     MBED_ASSERT(Init() == true);
00064     
00065     while(1) {
00066         
00067     }
00068 }
00069 
00070 
00071 bool Init() {
00072     pc.baud(PC_BAUDRATE);
00073     
00074     INT0.mode(PullDown);
00075     INT0.fall(dmpDataUpdate);
00076     
00077     mpu.initialize();
00078     if (mpu.testConnection()) {
00079         pc.puts("MPU6050 test connection passed.\n");
00080     } else {
00081         pc.puts("MPU6050 test connection failed.\n");
00082         return false;
00083     }
00084     if (mpu.dmpInitialize() == 0) {
00085         pc.puts("succeed in MPU6050 DMP Initializing.\n");
00086     } else {
00087         pc.puts("failed in MPU6050 DMP Initializing.\n");
00088         return false;
00089     }
00090     mpu.setXAccelOffset(offset.ax);
00091     mpu.setYAccelOffset(offset.ay);
00092     mpu.setZAccelOffset(offset.az);
00093     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00094     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00095     mpu.setXGyroOffsetUser(offset.gx);
00096     mpu.setYGyroOffsetUser(offset.gy);
00097     mpu.setZGyroOffsetUser(offset.gz);
00098     mpu.setDMPEnabled(true);    // Enable DMP
00099     packetSize = mpu.dmpGetFIFOPacketSize();
00100     dmpReady = true;    // Enable interrupt.
00101     
00102     pc.puts("Init finish!\n");
00103     
00104     return true;
00105 }
00106 
00107 
00108 void dmpDataUpdate() {
00109     // Check that this interrupt has enabled.
00110     if (dmpReady == false) return;
00111     
00112     mpuIntStatus = mpu.getIntStatus();
00113     fifoCount = mpu.getFIFOCount();
00114     
00115     // Check that this interrupt is a FIFO buffer overflow interrupt.
00116     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00117         mpu.resetFIFO();
00118         pc.puts("FIFO overflow!\n");
00119         return;
00120         
00121     // Check that this interrupt is a Data Ready interrupt.
00122     } else if (mpuIntStatus & 0x02) {
00123         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00124         
00125         mpu.getFIFOBytes(fifoBuffer, packetSize);
00126         
00127     #ifdef OUTPUT_QUATERNION
00128         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00129         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;
00130         pc.puts(snprintf_buffer);
00131     #endif
00132         
00133     #ifdef OUTPUT_EULER
00134         float euler[3];
00135         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00136         mpu.dmpGetEuler(euler, &dmpData.q);
00137         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;
00138         pc.puts(snprintf_buffer);
00139     #endif
00140         
00141     #ifdef OUTPUT_ROLL_PITCH_YAW
00142         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00143         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
00144         float rollPitchYaw[3];
00145         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
00146         dmpData.roll = rollPitchYaw[2];
00147         dmpData.pitch = rollPitchYaw[1];
00148         dmpData.yaw = rollPitchYaw[0];
00149         
00150         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;
00151         pc.puts(snprintf_buffer);
00152     #endif
00153         
00154     #ifdef OUTPUT_FOR_TEAPOT
00155         teapotPacket[2] = fifoBuffer[0];
00156         teapotPacket[3] = fifoBuffer[1];
00157         teapotPacket[4] = fifoBuffer[4];        
00158         teapotPacket[5] = fifoBuffer[5];
00159         teapotPacket[6] = fifoBuffer[8];
00160         teapotPacket[7] = fifoBuffer[9];
00161         teapotPacket[8] = fifoBuffer[12];
00162         teapotPacket[9] = fifoBuffer[13];
00163         for (uint8_t i = 0; i < 14; i++) {
00164             pc.putc(teapotPacket[i]);
00165         }
00166     #endif
00167         
00168     #ifdef OUTPUT_TEMPERATURE
00169         float temp = mpu.getTemperature() / 340.0 + 36.53;
00170         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
00171         pc.puts(snprintf_buffer);
00172     #endif
00173         
00174         pc.puts("\n");
00175     }
00176 }