mbed using PB8,PB9 to contorl I2C successfull dmpinitialize MPU6050
Dependencies: MPU6050_DMP_Nucleo-I2Cdev mbed
Fork of MPU9150_nucleo_i2cdev by
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?
User | Revision | Line number | New 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 | } |