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@4:01921a136f58, 2014-04-30 (annotated)

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