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@14:267368c83b6a, 2014-05-02 (annotated)

Committer:
pHysiX
Date:
Fri May 02 17:19:00 2014 +0000
Revision:
14:267368c83b6a
Parent:
12:953d25061417
Child:
16:9072cd6fa8d1
Reduced ESC pulse frequency to 200Hz from 400Hz since control is run up to 200Hz

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