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.

RTOS-Setup/src/setup.cpp@10:ef5fe86f67fe, 2014-05-01 (annotated)

Committer:
pHysiX
Date:
Thu May 01 23:50:21 2014 +0000
Revision:
10:ef5fe86f67fe
Parent:
9:371950017779
Child:
12:953d25061417
Fixed quaternion stability: FIFO was not serviced fast enough, and lacked correct tidying after each read

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 4:01921a136f58 4 Serial BT(p28, p27);
pHysiX 4:01921a136f58 5 DigitalOut BT_CMD(p29);
pHysiX 0:8c28fac22d27 6
pHysiX 10:ef5fe86f67fe 7 /* MPU6050 SDA, SCL (18, 19) */
pHysiX 0:8c28fac22d27 8 MPU6050 imu(p9, p10);
pHysiX 0:8c28fac22d27 9
pHysiX 3:605fbcb54e75 10 //DigitalOut LED[4] = {LED1, LED2, LED3, LED4};
pHysiX 3:605fbcb54e75 11
pHysiX 3:605fbcb54e75 12 //Kc, Ti, Td, interval
pHysiX 3:605fbcb54e75 13 PID yawPIDrate(KP_YAW_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 14 PID pitchPIDrate(KP_PITCH_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 15 PID rollPIDrate(KP_ROLL_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 16
pHysiX 3:605fbcb54e75 17 PwmOut ESC[4] = {p21, p22, p23, p24};
pHysiX 3:605fbcb54e75 18
pHysiX 3:605fbcb54e75 19
pHysiX 2:ab967d7b4346 20
pHysiX 3:605fbcb54e75 21 // ================================================================
pHysiX 3:605fbcb54e75 22 // === MAIN SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 23 // ================================================================
pHysiX 3:605fbcb54e75 24 bool setupALLdevices(void)
pHysiX 3:605fbcb54e75 25 {
pHysiX 3:605fbcb54e75 26 bool error = false;
pHysiX 10:ef5fe86f67fe 27
pHysiX 10:ef5fe86f67fe 28 box_demo = true;
pHysiX 3:605fbcb54e75 29
pHysiX 3:605fbcb54e75 30 if (!setup_ESC()) {
pHysiX 3:605fbcb54e75 31 imu.debugSerial.printf("ESC FAILED!!!\n");
pHysiX 3:605fbcb54e75 32 error = true;
pHysiX 3:605fbcb54e75 33 }
pHysiX 0:8c28fac22d27 34
pHysiX 3:605fbcb54e75 35 if (setup_bt())
pHysiX 3:605fbcb54e75 36 BT.printf("BT established!\n");
pHysiX 3:605fbcb54e75 37 else error = true;
pHysiX 3:605fbcb54e75 38
pHysiX 3:605fbcb54e75 39 if (setup_PID())
pHysiX 3:605fbcb54e75 40 BT.printf("PID established!\n");
pHysiX 3:605fbcb54e75 41 else error = true;
pHysiX 0:8c28fac22d27 42
pHysiX 3:605fbcb54e75 43 if (setup_mpu6050())
pHysiX 3:605fbcb54e75 44 BT.printf("MPU6050 established!\n");
pHysiX 3:605fbcb54e75 45 else error = true;
pHysiX 3:605fbcb54e75 46
pHysiX 3:605fbcb54e75 47 return error;
pHysiX 3:605fbcb54e75 48 }
pHysiX 0:8c28fac22d27 49
pHysiX 3:605fbcb54e75 50
pHysiX 3:605fbcb54e75 51
pHysiX 3:605fbcb54e75 52
pHysiX 3:605fbcb54e75 53 // ****************************************************************
pHysiX 3:605fbcb54e75 54 // === ESC SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 55 // ****************************************************************
pHysiX 2:ab967d7b4346 56 bool setup_ESC(void)
pHysiX 2:ab967d7b4346 57 {
pHysiX 3:605fbcb54e75 58 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 59 ESC[i].period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 60
pHysiX 3:605fbcb54e75 61 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 62 ESC[i].pulsewidth_us(0);
pHysiX 3:605fbcb54e75 63
pHysiX 2:ab967d7b4346 64 return true;
pHysiX 2:ab967d7b4346 65 }
pHysiX 2:ab967d7b4346 66
pHysiX 3:605fbcb54e75 67 // ****************************************************************
pHysiX 3:605fbcb54e75 68 // === BLUETOOTH SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 69 // ****************************************************************
pHysiX 0:8c28fac22d27 70 bool setup_bt(void)
pHysiX 0:8c28fac22d27 71 {
pHysiX 0:8c28fac22d27 72 BT.baud(115200);
pHysiX 0:8c28fac22d27 73 BT_CMD = 0;
pHysiX 0:8c28fac22d27 74 BT.printf("Bluetooth online!\n");
pHysiX 0:8c28fac22d27 75 return true;
pHysiX 0:8c28fac22d27 76 }
pHysiX 0:8c28fac22d27 77
pHysiX 3:605fbcb54e75 78 // ****************************************************************
pHysiX 3:605fbcb54e75 79 // === PID SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 80 // ****************************************************************
pHysiX 3:605fbcb54e75 81 bool setup_PID(void)
pHysiX 3:605fbcb54e75 82 {
pHysiX 9:371950017779 83 yawPIDrate.setInputLimits(-1100, 1100);
pHysiX 9:371950017779 84 yawPIDrate.setOutputLimits(-1000, 1000);
pHysiX 3:605fbcb54e75 85 yawPIDrate.setMode(AUTO_MODE);
pHysiX 10:ef5fe86f67fe 86
pHysiX 9:371950017779 87 pitchPIDrate.setInputLimits(-1100, 1100);
pHysiX 9:371950017779 88 pitchPIDrate.setOutputLimits(-1000, 1000);
pHysiX 3:605fbcb54e75 89 pitchPIDrate.setMode(AUTO_MODE);
pHysiX 3:605fbcb54e75 90
pHysiX 9:371950017779 91 rollPIDrate.setInputLimits(-1100, 1100);
pHysiX 9:371950017779 92 rollPIDrate.setOutputLimits(-1000, 1000);
pHysiX 3:605fbcb54e75 93 rollPIDrate.setMode(AUTO_MODE);
pHysiX 10:ef5fe86f67fe 94
pHysiX 3:605fbcb54e75 95 return true;
pHysiX 3:605fbcb54e75 96 }
pHysiX 3:605fbcb54e75 97
pHysiX 3:605fbcb54e75 98 // ****************************************************************
pHysiX 3:605fbcb54e75 99 // === MPU6050 SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 100 // ****************************************************************
pHysiX 3:605fbcb54e75 101 bool imu_available = false;
pHysiX 3:605fbcb54e75 102
pHysiX 3:605fbcb54e75 103 /*/ MPU control/status variables: */
pHysiX 3:605fbcb54e75 104 bool dmpReady = false; // set true if DMP init was successful
pHysiX 3:605fbcb54e75 105 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
pHysiX 3:605fbcb54e75 106 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
pHysiX 3:605fbcb54e75 107
pHysiX 0:8c28fac22d27 108 bool setup_mpu6050(void)
pHysiX 0:8c28fac22d27 109 {
pHysiX 0:8c28fac22d27 110 imu.reset();
pHysiX 0:8c28fac22d27 111 imu.debugSerial.baud(115200);
pHysiX 0:8c28fac22d27 112 wait_ms(5);
pHysiX 0:8c28fac22d27 113
pHysiX 0:8c28fac22d27 114 imu.initialize();
pHysiX 0:8c28fac22d27 115 imu_available = imu.testConnection();
pHysiX 0:8c28fac22d27 116
pHysiX 0:8c28fac22d27 117 imu.debugSerial.printf("IMU status...\t\t\t");
pHysiX 0:8c28fac22d27 118 imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
pHysiX 0:8c28fac22d27 119
pHysiX 0:8c28fac22d27 120 if (imu_available) {
pHysiX 0:8c28fac22d27 121 devStatus = imu.dmpInitialize();
pHysiX 0:8c28fac22d27 122
pHysiX 3:605fbcb54e75 123 // supply your own gyro offsets here, scaled for min sensitivity
pHysiX 4:01921a136f58 124 //imu.setXGyroOffset(55);
pHysiX 4:01921a136f58 125 //imu.setYGyroOffset(3);
pHysiX 4:01921a136f58 126 //imu.setZGyroOffset(-2);
pHysiX 3:605fbcb54e75 127 //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
pHysiX 0:8c28fac22d27 128
pHysiX 0:8c28fac22d27 129 if(!devStatus) {
pHysiX 0:8c28fac22d27 130 imu.setDMPEnabled(true);
pHysiX 0:8c28fac22d27 131 while (!imu.getIntDataReadyStatus());
pHysiX 0:8c28fac22d27 132 packetSize = imu.dmpGetFIFOPacketSize();
pHysiX 0:8c28fac22d27 133 dmpReady = true;
pHysiX 0:8c28fac22d27 134 } else {
pHysiX 0:8c28fac22d27 135 imu.debugSerial.printf("\tDMP setup failed!\n");
pHysiX 0:8c28fac22d27 136 return false;
pHysiX 0:8c28fac22d27 137 }
pHysiX 0:8c28fac22d27 138
pHysiX 0:8c28fac22d27 139 imu.resetFIFO();
pHysiX 0:8c28fac22d27 140 } else {
pHysiX 0:8c28fac22d27 141 return false;
pHysiX 0:8c28fac22d27 142 }
pHysiX 0:8c28fac22d27 143
pHysiX 0:8c28fac22d27 144 imu.debugSerial.printf("IMU setup routine done!");
pHysiX 0:8c28fac22d27 145
pHysiX 0:8c28fac22d27 146 return dmpReady;
pHysiX 0:8c28fac22d27 147 }
pHysiX 0:8c28fac22d27 148