Ian Hua / Quadcopter-mbedRTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

setup.cpp@0:8c28fac22d27, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 10:36:33 2014 +0000
Revision:
0:8c28fac22d27
MPU6050: DMP, RTOS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 0:8c28fac22d27 1 #include "setup.h"
pHysiX 0:8c28fac22d27 2 #include "tasks.h"
pHysiX 0:8c28fac22d27 3
pHysiX 0:8c28fac22d27 4 Serial BT(p13, p14);
pHysiX 0:8c28fac22d27 5 DigitalOut BT_CMD(p12);
pHysiX 0:8c28fac22d27 6
pHysiX 0:8c28fac22d27 7 MPU6050 imu(p9, p10);
pHysiX 0:8c28fac22d27 8
pHysiX 0:8c28fac22d27 9 DigitalOut led1(LED1);
pHysiX 0:8c28fac22d27 10 DigitalOut led2(LED2);
pHysiX 0:8c28fac22d27 11 DigitalOut led3(LED3);
pHysiX 0:8c28fac22d27 12 DigitalOut led4(LED4);
pHysiX 0:8c28fac22d27 13
pHysiX 0:8c28fac22d27 14 bool imu_available = false;
pHysiX 0:8c28fac22d27 15
pHysiX 0:8c28fac22d27 16 // MPU control/status vars
pHysiX 0:8c28fac22d27 17 bool dmpReady = false; // set true if DMP init was successful
pHysiX 0:8c28fac22d27 18 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
pHysiX 0:8c28fac22d27 19 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
pHysiX 0:8c28fac22d27 20
pHysiX 0:8c28fac22d27 21 bool setup_led(void)
pHysiX 0:8c28fac22d27 22 {
pHysiX 0:8c28fac22d27 23 ledReset();
pHysiX 0:8c28fac22d27 24 return true;
pHysiX 0:8c28fac22d27 25 }
pHysiX 0:8c28fac22d27 26
pHysiX 0:8c28fac22d27 27 bool setup_bt(void)
pHysiX 0:8c28fac22d27 28 {
pHysiX 0:8c28fac22d27 29 BT.baud(115200);
pHysiX 0:8c28fac22d27 30 BT_CMD = 0;
pHysiX 0:8c28fac22d27 31 BT.printf("Bluetooth online!\n");
pHysiX 0:8c28fac22d27 32 return true;
pHysiX 0:8c28fac22d27 33 }
pHysiX 0:8c28fac22d27 34
pHysiX 0:8c28fac22d27 35 bool setup_mpu6050(void)
pHysiX 0:8c28fac22d27 36 {
pHysiX 0:8c28fac22d27 37 imu.reset();
pHysiX 0:8c28fac22d27 38 imu.debugSerial.baud(115200);
pHysiX 0:8c28fac22d27 39 wait_ms(5);
pHysiX 0:8c28fac22d27 40
pHysiX 0:8c28fac22d27 41 imu.initialize();
pHysiX 0:8c28fac22d27 42 imu_available = imu.testConnection();
pHysiX 0:8c28fac22d27 43
pHysiX 0:8c28fac22d27 44 imu.debugSerial.printf("IMU status...\t\t\t");
pHysiX 0:8c28fac22d27 45 imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
pHysiX 0:8c28fac22d27 46
pHysiX 0:8c28fac22d27 47 if (imu_available) {
pHysiX 0:8c28fac22d27 48 devStatus = imu.dmpInitialize();
pHysiX 0:8c28fac22d27 49
pHysiX 0:8c28fac22d27 50 /*
pHysiX 0:8c28fac22d27 51 // supply your own gyro offsets here, scaled for min sensitivity
pHysiX 0:8c28fac22d27 52 mpu.setXGyroOffset(-61);
pHysiX 0:8c28fac22d27 53 mpu.setYGyroOffset(-127);
pHysiX 0:8c28fac22d27 54 mpu.setZGyroOffset(19);
pHysiX 0:8c28fac22d27 55 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
pHysiX 0:8c28fac22d27 56 */
pHysiX 0:8c28fac22d27 57
pHysiX 0:8c28fac22d27 58 if(!devStatus) {
pHysiX 0:8c28fac22d27 59 imu.setDMPEnabled(true);
pHysiX 0:8c28fac22d27 60 while (!imu.getIntDataReadyStatus());
pHysiX 0:8c28fac22d27 61 packetSize = imu.dmpGetFIFOPacketSize();
pHysiX 0:8c28fac22d27 62 dmpReady = true;
pHysiX 0:8c28fac22d27 63 } else {
pHysiX 0:8c28fac22d27 64 imu.debugSerial.printf("\tDMP setup failed!\n");
pHysiX 0:8c28fac22d27 65 return false;
pHysiX 0:8c28fac22d27 66 }
pHysiX 0:8c28fac22d27 67
pHysiX 0:8c28fac22d27 68 imu.resetFIFO();
pHysiX 0:8c28fac22d27 69 } else {
pHysiX 0:8c28fac22d27 70 return false;
pHysiX 0:8c28fac22d27 71 }
pHysiX 0:8c28fac22d27 72
pHysiX 0:8c28fac22d27 73 imu.debugSerial.printf("IMU setup routine done!");
pHysiX 0:8c28fac22d27 74
pHysiX 0:8c28fac22d27 75 return dmpReady;
pHysiX 0:8c28fac22d27 76 }
pHysiX 0:8c28fac22d27 77