Weber Yang / Mbed 2 deprecated MPU9150_nucleo_i2cdev__

mbed using PB8,PB9 to contorl I2C successfull dmpinitialize MPU6050

Dependencies:   MPU6050_DMP_Nucleo-I2Cdev mbed

Fork of MPU9150_nucleo_i2cdev by Weber Yang

main.cpp@2:bc8df2b3a253, 2018-10-26 (annotated)

Committer:
WeberYang
Date:
Fri Oct 26 10:00:02 2018 +0000
Revision:
2:bc8df2b3a253
Parent:
1:ca8638ade4ab 
ok can use on the NUCLEOF103RB

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
WeberYang 1:ca8638ade4ab 41 Serial pc(USBTX, USBRX);
akashvibhute 0:706ce7540c8f 42
akashvibhute 0:706ce7540c8f 43
akashvibhute 0:706ce7540c8f 44 MPU6050 mpu(PB_9, PB_8); // sda, scl pin
WeberYang 1:ca8638ade4ab 45 InterruptIn INT0(PA_9); // 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
WeberYang 1:ca8638ade4ab 65 VectorInt16 acc; // g
WeberYang 1:ca8638ade4ab 66 VectorInt16 gryo; // g
akashvibhute 0:706ce7540c8f 67 float roll, pitch, yaw; // rad
akashvibhute 0:706ce7540c8f 68 }dmpData;
akashvibhute 0:706ce7540c8f 69
akashvibhute 0:706ce7540c8f 70 bool Init();
akashvibhute 0:706ce7540c8f 71 void dmpDataUpdate();
akashvibhute 0:706ce7540c8f 72
akashvibhute 0:706ce7540c8f 73
akashvibhute 0:706ce7540c8f 74 int main() {
akashvibhute 0:706ce7540c8f 75 MBED_ASSERT(Init() == true);
akashvibhute 0:706ce7540c8f 76
akashvibhute 0:706ce7540c8f 77 while(1) {
akashvibhute 0:706ce7540c8f 78
akashvibhute 0:706ce7540c8f 79 }
akashvibhute 0:706ce7540c8f 80 }
akashvibhute 0:706ce7540c8f 81
akashvibhute 0:706ce7540c8f 82
akashvibhute 0:706ce7540c8f 83 bool Init() {
akashvibhute 0:706ce7540c8f 84 pc.baud(PC_BAUDRATE);
akashvibhute 0:706ce7540c8f 85
akashvibhute 0:706ce7540c8f 86 INT0.mode(PullDown);
akashvibhute 0:706ce7540c8f 87 INT0.fall(dmpDataUpdate);
akashvibhute 0:706ce7540c8f 88
akashvibhute 0:706ce7540c8f 89 mpu.initialize();
akashvibhute 0:706ce7540c8f 90 if (mpu.testConnection()) {
WeberYang 1:ca8638ade4ab 91 pc.printf("MPU6050 test connection passed.\n");
akashvibhute 0:706ce7540c8f 92 } else {
WeberYang 1:ca8638ade4ab 93 pc.printf("MPU6050 test connection failed.\n");
akashvibhute 0:706ce7540c8f 94 return false;
akashvibhute 0:706ce7540c8f 95 }
akashvibhute 0:706ce7540c8f 96 if (mpu.dmpInitialize() == 0) {
WeberYang 1:ca8638ade4ab 97 pc.printf("succeed in MPU6050 DMP Initializing.\n");
akashvibhute 0:706ce7540c8f 98 } else {
WeberYang 1:ca8638ade4ab 99 pc.printf("failed in MPU6050 DMP Initializing.\n");
akashvibhute 0:706ce7540c8f 100 return false;
akashvibhute 0:706ce7540c8f 101 }
akashvibhute 0:706ce7540c8f 102 mpu.setXAccelOffset(offset.ax);
akashvibhute 0:706ce7540c8f 103 mpu.setYAccelOffset(offset.ay);
akashvibhute 0:706ce7540c8f 104 mpu.setZAccelOffset(offset.az);
akashvibhute 0:706ce7540c8f 105 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
akashvibhute 0:706ce7540c8f 106 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
akashvibhute 0:706ce7540c8f 107 mpu.setXGyroOffsetUser(offset.gx);
akashvibhute 0:706ce7540c8f 108 mpu.setYGyroOffsetUser(offset.gy);
akashvibhute 0:706ce7540c8f 109 mpu.setZGyroOffsetUser(offset.gz);
akashvibhute 0:706ce7540c8f 110 mpu.setDMPEnabled(true); // Enable DMP
akashvibhute 0:706ce7540c8f 111 packetSize = mpu.dmpGetFIFOPacketSize();
akashvibhute 0:706ce7540c8f 112 dmpReady = true; // Enable interrupt.
akashvibhute 0:706ce7540c8f 113
WeberYang 1:ca8638ade4ab 114 pc.printf("Init finish!\n");
akashvibhute 0:706ce7540c8f 115
akashvibhute 0:706ce7540c8f 116 return true;
akashvibhute 0:706ce7540c8f 117 }
akashvibhute 0:706ce7540c8f 118
akashvibhute 0:706ce7540c8f 119
akashvibhute 0:706ce7540c8f 120 void dmpDataUpdate() {
akashvibhute 0:706ce7540c8f 121 // Check that this interrupt has enabled.
akashvibhute 0:706ce7540c8f 122 if (dmpReady == false) return;
akashvibhute 0:706ce7540c8f 123
akashvibhute 0:706ce7540c8f 124 mpuIntStatus = mpu.getIntStatus();
akashvibhute 0:706ce7540c8f 125 fifoCount = mpu.getFIFOCount();
akashvibhute 0:706ce7540c8f 126
akashvibhute 0:706ce7540c8f 127 // Check that this interrupt is a FIFO buffer overflow interrupt.
akashvibhute 0:706ce7540c8f 128 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
akashvibhute 0:706ce7540c8f 129 mpu.resetFIFO();
WeberYang 1:ca8638ade4ab 130 pc.printf("FIFO overflow!\n");
akashvibhute 0:706ce7540c8f 131 return;
akashvibhute 0:706ce7540c8f 132
akashvibhute 0:706ce7540c8f 133 // Check that this interrupt is a Data Ready interrupt.
akashvibhute 0:706ce7540c8f 134 } else if (mpuIntStatus & 0x02) {
akashvibhute 0:706ce7540c8f 135 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
akashvibhute 0:706ce7540c8f 136
akashvibhute 0:706ce7540c8f 137 mpu.getFIFOBytes(fifoBuffer, packetSize);
akashvibhute 0:706ce7540c8f 138
akashvibhute 0:706ce7540c8f 139 #ifdef OUTPUT_QUATERNION
akashvibhute 0:706ce7540c8f 140 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
akashvibhute 0:706ce7540c8f 141 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;
WeberYang 1:ca8638ade4ab 142 pc.printf(snprintf_buffer);
akashvibhute 0:706ce7540c8f 143 #endif
akashvibhute 0:706ce7540c8f 144
akashvibhute 0:706ce7540c8f 145 #ifdef OUTPUT_EULER
akashvibhute 0:706ce7540c8f 146 float euler[3];
akashvibhute 0:706ce7540c8f 147 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
akashvibhute 0:706ce7540c8f 148 mpu.dmpGetEuler(euler, &dmpData.q);
akashvibhute 0:706ce7540c8f 149 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;
WeberYang 1:ca8638ade4ab 150 pc.printf(snprintf_buffer);
akashvibhute 0:706ce7540c8f 151 #endif
akashvibhute 0:706ce7540c8f 152
akashvibhute 0:706ce7540c8f 153 #ifdef OUTPUT_ROLL_PITCH_YAW
akashvibhute 0:706ce7540c8f 154 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
akashvibhute 0:706ce7540c8f 155 mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
WeberYang 1:ca8638ade4ab 156 mpu.dmpGetAccel(&dmpData.acc, fifoBuffer);
akashvibhute 0:706ce7540c8f 157 float rollPitchYaw[3];
akashvibhute 0:706ce7540c8f 158 mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
akashvibhute 0:706ce7540c8f 159 dmpData.roll = rollPitchYaw[2];
akashvibhute 0:706ce7540c8f 160 dmpData.pitch = rollPitchYaw[1];
akashvibhute 0:706ce7540c8f 161 dmpData.yaw = rollPitchYaw[0];
WeberYang 1:ca8638ade4ab 162 pc.printf("ax = %d,ay = %d,az = %d\r\n",dmpData.acc.x,dmpData.acc.y,dmpData.acc.z);
WeberYang 1:ca8638ade4ab 163 // 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;
WeberYang 1:ca8638ade4ab 164 // pc.printf(snprintf_buffer);
akashvibhute 0:706ce7540c8f 165 #endif
akashvibhute 0:706ce7540c8f 166
akashvibhute 0:706ce7540c8f 167 #ifdef OUTPUT_FOR_TEAPOT
akashvibhute 0:706ce7540c8f 168 teapotPacket[2] = fifoBuffer[0];
akashvibhute 0:706ce7540c8f 169 teapotPacket[3] = fifoBuffer[1];
akashvibhute 0:706ce7540c8f 170 teapotPacket[4] = fifoBuffer[4];
akashvibhute 0:706ce7540c8f 171 teapotPacket[5] = fifoBuffer[5];
akashvibhute 0:706ce7540c8f 172 teapotPacket[6] = fifoBuffer[8];
akashvibhute 0:706ce7540c8f 173 teapotPacket[7] = fifoBuffer[9];
akashvibhute 0:706ce7540c8f 174 teapotPacket[8] = fifoBuffer[12];
akashvibhute 0:706ce7540c8f 175 teapotPacket[9] = fifoBuffer[13];
akashvibhute 0:706ce7540c8f 176 for (uint8_t i = 0; i < 14; i++) {
akashvibhute 0:706ce7540c8f 177 pc.putc(teapotPacket[i]);
akashvibhute 0:706ce7540c8f 178 }
akashvibhute 0:706ce7540c8f 179 #endif
akashvibhute 0:706ce7540c8f 180
akashvibhute 0:706ce7540c8f 181 #ifdef OUTPUT_TEMPERATURE
akashvibhute 0:706ce7540c8f 182 float temp = mpu.getTemperature() / 340.0 + 36.53;
akashvibhute 0:706ce7540c8f 183 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
WeberYang 1:ca8638ade4ab 184 pc.printf(snprintf_buffer);
akashvibhute 0:706ce7540c8f 185 #endif
akashvibhute 0:706ce7540c8f 186
WeberYang 1:ca8638ade4ab 187 pc.printf("\n");
akashvibhute 0:706ce7540c8f 188 }
akashvibhute 0:706ce7540c8f 189 }