Akash Vibhute / Mbed 2 deprecated MPU9150_nucleo_i2cdev

MPU DMP code modified for use with ST Nucelo boards. This program makes use of I2Cdev based DMP library.

Dependencies:   MPU6050_DMP_Nucleo-I2Cdev mbed

main.cpp@0:706ce7540c8f, 2015-03-29 (annotated)

Committer:
akashvibhute
Date:
Sun Mar 29 08:50:27 2015 +0000
Revision:
0:706ce7540c8f
MPU DMP code modified for use with ST Nucelo boards. This program makes use of I2Cdev based DMP library.

Who changed what in which revision?

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