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  *
00016  *  Modified by Akash Vibhute on 29 March 2015
00017  *  This program now works well with Nucelo boards from ST
00018  *
00019  */
00020 
00021 
00022 
00023 // Define Necessary.
00024 //#define OUTPUT_QUATERNION
00025 //#define OUTPUT_EULER
00026 #define OUTPUT_ROLL_PITCH_YAW
00027 //#define OUTPUT_FOR_TEAPOT
00028 //#define OUTPUT_TEMPERATURE
00029 
00030 
00031 #include "MPU6050_6Axis_MotionApps20.h"
00032 #include "mbed.h"
00033 #include "config.h"
00034 #include <stdio.h>
00035 
00036 
00037 #define DEG_TO_RAD(x) ( x * 0.01745329 )
00038 #define RAD_TO_DEG(x) ( x * 57.29578 )
00039 
00040 
00041 Serial pc(USBTX, USBRX);
00042 
00043 
00044 MPU6050 mpu(PB_9, PB_8);     // sda, scl pin
00045 InterruptIn INT0(PA_9);     // INT0 pin
00046 
00047 const int FIFO_BUFFER_SIZE = 128;
00048 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
00049 uint16_t fifoCount;
00050 uint16_t packetSize;
00051 bool dmpReady;
00052 uint8_t mpuIntStatus;
00053 const int snprintf_buffer_size = 100;
00054 char snprintf_buffer[snprintf_buffer_size];
00055 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00056 
00057 struct Offset {
00058     int16_t ax, ay, az;
00059     int16_t gx, gy, gz;
00060 }offset = {150, -350, 1000, -110, 5, 0};    // Measured values
00061 
00062 struct MPU6050_DmpData {
00063     Quaternion q;
00064     VectorFloat gravity;    // g
00065     VectorInt16 acc;        // g
00066     VectorInt16 gryo;    // g
00067     float roll, pitch, yaw;     // rad
00068 }dmpData;
00069 
00070 bool Init();
00071 void dmpDataUpdate();
00072 
00073 
00074 int main() {
00075     MBED_ASSERT(Init() == true);
00076     
00077     while(1) {
00078         
00079     }
00080 }
00081 
00082 
00083 bool Init() {
00084     pc.baud(PC_BAUDRATE);
00085     
00086     INT0.mode(PullDown);
00087     INT0.fall(dmpDataUpdate);
00088     
00089     mpu.initialize();
00090     if (mpu.testConnection()) {
00091         pc.printf("MPU6050 test connection passed.\n");
00092     } else {
00093         pc.printf("MPU6050 test connection failed.\n");
00094         return false;
00095     }
00096     if (mpu.dmpInitialize() == 0) {
00097         pc.printf("succeed in MPU6050 DMP Initializing.\n");
00098     } else {
00099         pc.printf("failed in MPU6050 DMP Initializing.\n");
00100         return false;
00101     }
00102     mpu.setXAccelOffset(offset.ax);
00103     mpu.setYAccelOffset(offset.ay);
00104     mpu.setZAccelOffset(offset.az);
00105     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00106     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00107     mpu.setXGyroOffsetUser(offset.gx);
00108     mpu.setYGyroOffsetUser(offset.gy);
00109     mpu.setZGyroOffsetUser(offset.gz);
00110     mpu.setDMPEnabled(true);    // Enable DMP
00111     packetSize = mpu.dmpGetFIFOPacketSize();
00112     dmpReady = true;    // Enable interrupt.
00113     
00114     pc.printf("Init finish!\n");
00115     
00116     return true;
00117 }
00118 
00119 
00120 void dmpDataUpdate() {
00121     // Check that this interrupt has enabled.
00122     if (dmpReady == false) return;
00123     
00124     mpuIntStatus = mpu.getIntStatus();
00125     fifoCount = mpu.getFIFOCount();
00126     
00127     // Check that this interrupt is a FIFO buffer overflow interrupt.
00128     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00129         mpu.resetFIFO();
00130         pc.printf("FIFO overflow!\n");
00131         return;
00132         
00133     // Check that this interrupt is a Data Ready interrupt.
00134     } else if (mpuIntStatus & 0x02) {
00135         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00136         
00137         mpu.getFIFOBytes(fifoBuffer, packetSize);
00138         
00139     #ifdef OUTPUT_QUATERNION
00140         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00141         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;
00142         pc.printf(snprintf_buffer);
00143     #endif
00144         
00145     #ifdef OUTPUT_EULER
00146         float euler[3];
00147         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00148         mpu.dmpGetEuler(euler, &dmpData.q);
00149         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;
00150         pc.printf(snprintf_buffer);
00151     #endif
00152         
00153     #ifdef OUTPUT_ROLL_PITCH_YAW
00154         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00155         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
00156         mpu.dmpGetAccel(&dmpData.acc, fifoBuffer);
00157         float rollPitchYaw[3];
00158         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
00159         dmpData.roll = rollPitchYaw[2];
00160         dmpData.pitch = rollPitchYaw[1];
00161         dmpData.yaw = rollPitchYaw[0];
00162         pc.printf("ax = %d,ay = %d,az = %d\r\n",dmpData.acc.x,dmpData.acc.y,dmpData.acc.z);
00163 //        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;
00164 //        pc.printf(snprintf_buffer);
00165     #endif
00166         
00167     #ifdef OUTPUT_FOR_TEAPOT
00168         teapotPacket[2] = fifoBuffer[0];
00169         teapotPacket[3] = fifoBuffer[1];
00170         teapotPacket[4] = fifoBuffer[4];        
00171         teapotPacket[5] = fifoBuffer[5];
00172         teapotPacket[6] = fifoBuffer[8];
00173         teapotPacket[7] = fifoBuffer[9];
00174         teapotPacket[8] = fifoBuffer[12];
00175         teapotPacket[9] = fifoBuffer[13];
00176         for (uint8_t i = 0; i < 14; i++) {
00177             pc.putc(teapotPacket[i]);
00178         }
00179     #endif
00180         
00181     #ifdef OUTPUT_TEMPERATURE
00182         float temp = mpu.getTemperature() / 340.0 + 36.53;
00183         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
00184         pc.printf(snprintf_buffer);
00185     #endif
00186         
00187         pc.printf("\n");
00188     }
00189 }