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@25:a7cfe421cb4a, 2014-05-08 (annotated)

Committer:
pHysiX
Date:
Thu May 08 13:35:13 2014 +0000
Revision:
25:a7cfe421cb4a
Parent:
24:54a8cdf17378
Child:
27:18b6580eb0b1
Added RATE and STABLE mode switch.; Tuned Primary and Secondary PID loops.; Quadcopter can now be flown stably indoors.; Need to add in Integral Control, and then clean up to move away from Bluetooth arming.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 22:ef8aa9728013 1 /* File: Setup.h
pHysiX 22:ef8aa9728013 2 * Author: Trung Tin Ian HUA
pHysiX 22:ef8aa9728013 3 * Date: May 2014
pHysiX 22:ef8aa9728013 4 * Purpose: Setup code to initialise all device
pHysiX 22:ef8aa9728013 5 */
pHysiX 22:ef8aa9728013 6
pHysiX 25:a7cfe421cb4a 7 float KP_PITCH_STABLE = 0.8; // 0.5;
pHysiX 25:a7cfe421cb4a 8 float KP_ROLL_STABLE = 0.8; // 0.5;
pHysiX 25:a7cfe421cb4a 9 float KP_YAW_RATE = 8.8; // 8.5 for RATE MODE
pHysiX 25:a7cfe421cb4a 10 float KP_PITCH_RATE = 7.6; // 8.5 for RATE MODE
pHysiX 25:a7cfe421cb4a 11 float KP_ROLL_RATE = 7.6; // 8.5 for RATE MODE
pHysiX 21:b642c18eccd1 12
pHysiX 21:b642c18eccd1 13 float PID_TI_RATE = 0.0;
pHysiX 21:b642c18eccd1 14 float PID_TI_STABLE = 0.0;
pHysiX 21:b642c18eccd1 15
pHysiX 0:8c28fac22d27 16 #include "setup.h"
pHysiX 0:8c28fac22d27 17 #include "tasks.h"
pHysiX 0:8c28fac22d27 18
pHysiX 4:01921a136f58 19 Serial BT(p28, p27);
pHysiX 4:01921a136f58 20 DigitalOut BT_CMD(p29);
pHysiX 22:ef8aa9728013 21 MPU6050 imu(p9, p10); // MPU6050-Tilty SDA, SCL (18, 19)
pHysiX 22:ef8aa9728013 22 MPL3115A2 altimeter(p9, p10);
pHysiX 12:953d25061417 23 #ifdef ENABLE_COMPASS
pHysiX 12:953d25061417 24 HMC5883L compass(p9, p10);
pHysiX 12:953d25061417 25 #endif
pHysiX 12:953d25061417 26
pHysiX 22:ef8aa9728013 27 PID pitchPIDstable(KP_PITCH_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);
pHysiX 22:ef8aa9728013 28 PID rollPIDstable(KP_ROLL_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);
pHysiX 12:953d25061417 29
pHysiX 21:b642c18eccd1 30 PID yawPIDrate(KP_YAW_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
pHysiX 21:b642c18eccd1 31 PID pitchPIDrate(KP_PITCH_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
pHysiX 21:b642c18eccd1 32 PID rollPIDrate(KP_ROLL_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
pHysiX 20:b193a50a2ba3 33
pHysiX 3:605fbcb54e75 34 PwmOut ESC[4] = {p21, p22, p23, p24};
pHysiX 3:605fbcb54e75 35
pHysiX 3:605fbcb54e75 36
pHysiX 2:ab967d7b4346 37
pHysiX 3:605fbcb54e75 38 // ================================================================
pHysiX 3:605fbcb54e75 39 // === MAIN SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 40 // ================================================================
pHysiX 3:605fbcb54e75 41 bool setupALLdevices(void)
pHysiX 3:605fbcb54e75 42 {
pHysiX 3:605fbcb54e75 43 bool error = false;
pHysiX 22:ef8aa9728013 44 box_demo = false;
pHysiX 25:a7cfe421cb4a 45 mode = STABLE;
pHysiX 3:605fbcb54e75 46
pHysiX 3:605fbcb54e75 47 if (!setup_ESC()) {
pHysiX 3:605fbcb54e75 48 imu.debugSerial.printf("ESC FAILED!!!\n");
pHysiX 3:605fbcb54e75 49 error = true;
pHysiX 3:605fbcb54e75 50 }
pHysiX 0:8c28fac22d27 51
pHysiX 3:605fbcb54e75 52 if (setup_bt())
pHysiX 21:b642c18eccd1 53 imu.debugSerial.printf("BT established!\n");
pHysiX 3:605fbcb54e75 54 else error = true;
pHysiX 3:605fbcb54e75 55
pHysiX 3:605fbcb54e75 56 if (setup_PID())
pHysiX 21:b642c18eccd1 57 imu.debugSerial.printf("PID established!\n");
pHysiX 3:605fbcb54e75 58 else error = true;
pHysiX 0:8c28fac22d27 59
pHysiX 3:605fbcb54e75 60 if (setup_mpu6050())
pHysiX 21:b642c18eccd1 61 imu.debugSerial.printf("MPU6050 established!\n");
pHysiX 3:605fbcb54e75 62 else error = true;
pHysiX 3:605fbcb54e75 63
pHysiX 22:ef8aa9728013 64 if (setup_altimeter())
pHysiX 22:ef8aa9728013 65 imu.debugSerial.printf("Altimeter established!\n");
pHysiX 22:ef8aa9728013 66 else {
pHysiX 22:ef8aa9728013 67 error = true;
pHysiX 22:ef8aa9728013 68 imu.debugSerial.printf("ALTIMETER FAILED\n");
pHysiX 22:ef8aa9728013 69 }
pHysiX 22:ef8aa9728013 70
pHysiX 12:953d25061417 71 #ifdef ENABLE_COMPASS
pHysiX 12:953d25061417 72 if (setup_compass())
pHysiX 21:b642c18eccd1 73 imu.debugSerial.printf("Compass established!\n");
pHysiX 12:953d25061417 74 else error = true;
pHysiX 12:953d25061417 75 #endif
pHysiX 12:953d25061417 76
pHysiX 3:605fbcb54e75 77 return error;
pHysiX 3:605fbcb54e75 78 }
pHysiX 0:8c28fac22d27 79
pHysiX 3:605fbcb54e75 80
pHysiX 3:605fbcb54e75 81
pHysiX 3:605fbcb54e75 82
pHysiX 3:605fbcb54e75 83 // ****************************************************************
pHysiX 3:605fbcb54e75 84 // === ESC SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 85 // ****************************************************************
pHysiX 2:ab967d7b4346 86 bool setup_ESC(void)
pHysiX 2:ab967d7b4346 87 {
pHysiX 3:605fbcb54e75 88 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 89 ESC[i].period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 90
pHysiX 3:605fbcb54e75 91 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 92 ESC[i].pulsewidth_us(0);
pHysiX 3:605fbcb54e75 93
pHysiX 14:267368c83b6a 94 armed = false;
pHysiX 14:267368c83b6a 95
pHysiX 2:ab967d7b4346 96 return true;
pHysiX 2:ab967d7b4346 97 }
pHysiX 2:ab967d7b4346 98
pHysiX 3:605fbcb54e75 99 // ****************************************************************
pHysiX 3:605fbcb54e75 100 // === BLUETOOTH SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 101 // ****************************************************************
pHysiX 0:8c28fac22d27 102 bool setup_bt(void)
pHysiX 0:8c28fac22d27 103 {
pHysiX 0:8c28fac22d27 104 BT.baud(115200);
pHysiX 22:ef8aa9728013 105 BT_CMD = 0; // Place bluetooth into normal mode
pHysiX 0:8c28fac22d27 106 BT.printf("Bluetooth online!\n");
pHysiX 0:8c28fac22d27 107 return true;
pHysiX 0:8c28fac22d27 108 }
pHysiX 0:8c28fac22d27 109
pHysiX 3:605fbcb54e75 110 // ****************************************************************
pHysiX 3:605fbcb54e75 111 // === PID SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 112 // ****************************************************************
pHysiX 3:605fbcb54e75 113 bool setup_PID(void)
pHysiX 3:605fbcb54e75 114 {
pHysiX 21:b642c18eccd1 115 pitchPIDstable.setInputLimits(-90.0, 90.0);
pHysiX 24:54a8cdf17378 116 pitchPIDstable.setOutputLimits(-250, 250.0);
pHysiX 21:b642c18eccd1 117 pitchPIDstable.setBias(0.0);
pHysiX 21:b642c18eccd1 118 pitchPIDstable.setMode(AUTO_MODE);
pHysiX 22:ef8aa9728013 119
pHysiX 21:b642c18eccd1 120 rollPIDstable.setInputLimits(-90.0, 90.0);
pHysiX 24:54a8cdf17378 121 rollPIDstable.setOutputLimits(-250, 250.0);
pHysiX 21:b642c18eccd1 122 rollPIDstable.setBias(0.0);
pHysiX 21:b642c18eccd1 123 rollPIDstable.setMode(AUTO_MODE);
pHysiX 22:ef8aa9728013 124
pHysiX 21:b642c18eccd1 125 yawPIDrate.setInputLimits(-500.0, 500.0);
pHysiX 21:b642c18eccd1 126 yawPIDrate.setOutputLimits(-200.0, 200.0);
pHysiX 21:b642c18eccd1 127 yawPIDrate.setBias(0.0);
pHysiX 3:605fbcb54e75 128 yawPIDrate.setMode(AUTO_MODE);
pHysiX 12:953d25061417 129
pHysiX 21:b642c18eccd1 130 pitchPIDrate.setInputLimits(-500.0, 500.0);
pHysiX 21:b642c18eccd1 131 pitchPIDrate.setOutputLimits(-200.0, 200.0);
pHysiX 21:b642c18eccd1 132 pitchPIDrate.setBias(0.0);
pHysiX 3:605fbcb54e75 133 pitchPIDrate.setMode(AUTO_MODE);
pHysiX 22:ef8aa9728013 134
pHysiX 21:b642c18eccd1 135 rollPIDrate.setInputLimits(-500.0, 500.0);
pHysiX 21:b642c18eccd1 136 rollPIDrate.setOutputLimits(-200.0, 200.0);
pHysiX 21:b642c18eccd1 137 rollPIDrate.setBias(0.0);
pHysiX 3:605fbcb54e75 138 rollPIDrate.setMode(AUTO_MODE);
pHysiX 24:54a8cdf17378 139
pHysiX 24:54a8cdf17378 140 pitchPIDstable.reset();
pHysiX 24:54a8cdf17378 141 rollPIDstable.reset();
pHysiX 24:54a8cdf17378 142 yawPIDrate.reset();
pHysiX 24:54a8cdf17378 143 pitchPIDrate.reset();
pHysiX 24:54a8cdf17378 144 rollPIDrate.reset();
pHysiX 12:953d25061417 145
pHysiX 3:605fbcb54e75 146 return true;
pHysiX 3:605fbcb54e75 147 }
pHysiX 3:605fbcb54e75 148
pHysiX 3:605fbcb54e75 149 // ****************************************************************
pHysiX 3:605fbcb54e75 150 // === MPU6050 SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 151 // ****************************************************************
pHysiX 3:605fbcb54e75 152 bool imu_available = false;
pHysiX 3:605fbcb54e75 153
pHysiX 3:605fbcb54e75 154 /*/ MPU control/status variables: */
pHysiX 3:605fbcb54e75 155 bool dmpReady = false; // set true if DMP init was successful
pHysiX 3:605fbcb54e75 156 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
pHysiX 3:605fbcb54e75 157 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
pHysiX 3:605fbcb54e75 158
pHysiX 0:8c28fac22d27 159 bool setup_mpu6050(void)
pHysiX 0:8c28fac22d27 160 {
pHysiX 0:8c28fac22d27 161 imu.reset();
pHysiX 0:8c28fac22d27 162 imu.debugSerial.baud(115200);
pHysiX 0:8c28fac22d27 163 wait_ms(5);
pHysiX 0:8c28fac22d27 164
pHysiX 0:8c28fac22d27 165 imu.initialize();
pHysiX 0:8c28fac22d27 166 imu_available = imu.testConnection();
pHysiX 0:8c28fac22d27 167
pHysiX 0:8c28fac22d27 168 imu.debugSerial.printf("IMU status...\t\t\t");
pHysiX 0:8c28fac22d27 169 imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
pHysiX 0:8c28fac22d27 170
pHysiX 0:8c28fac22d27 171 if (imu_available) {
pHysiX 0:8c28fac22d27 172 devStatus = imu.dmpInitialize();
pHysiX 0:8c28fac22d27 173
pHysiX 3:605fbcb54e75 174 // supply your own gyro offsets here, scaled for min sensitivity
pHysiX 4:01921a136f58 175 //imu.setXGyroOffset(55);
pHysiX 4:01921a136f58 176 //imu.setYGyroOffset(3);
pHysiX 4:01921a136f58 177 //imu.setZGyroOffset(-2);
pHysiX 3:605fbcb54e75 178 //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
pHysiX 0:8c28fac22d27 179
pHysiX 0:8c28fac22d27 180 if(!devStatus) {
pHysiX 0:8c28fac22d27 181 imu.setDMPEnabled(true);
pHysiX 0:8c28fac22d27 182 while (!imu.getIntDataReadyStatus());
pHysiX 0:8c28fac22d27 183 packetSize = imu.dmpGetFIFOPacketSize();
pHysiX 16:9072cd6fa8d1 184 imu.debugSerial.printf("Packet Size: %d\n", packetSize);
pHysiX 0:8c28fac22d27 185 dmpReady = true;
pHysiX 16:9072cd6fa8d1 186
pHysiX 16:9072cd6fa8d1 187 int8_t xgOffsetTC = imu.getXGyroOffset();
pHysiX 16:9072cd6fa8d1 188 int8_t ygOffsetTC = imu.getYGyroOffset();
pHysiX 16:9072cd6fa8d1 189 int8_t zgOffsetTC = imu.getZGyroOffset();
pHysiX 16:9072cd6fa8d1 190
pHysiX 16:9072cd6fa8d1 191 imu.debugSerial.printf("X Offset: %4d Y Offset: %4d Z Offset: %4d\n", xgOffsetTC, ygOffsetTC, zgOffsetTC);
pHysiX 16:9072cd6fa8d1 192
pHysiX 0:8c28fac22d27 193 } else {
pHysiX 0:8c28fac22d27 194 imu.debugSerial.printf("\tDMP setup failed!\n");
pHysiX 0:8c28fac22d27 195 return false;
pHysiX 0:8c28fac22d27 196 }
pHysiX 0:8c28fac22d27 197
pHysiX 0:8c28fac22d27 198 imu.resetFIFO();
pHysiX 0:8c28fac22d27 199 } else {
pHysiX 0:8c28fac22d27 200 return false;
pHysiX 0:8c28fac22d27 201 }
pHysiX 0:8c28fac22d27 202
pHysiX 0:8c28fac22d27 203 imu.debugSerial.printf("IMU setup routine done!");
pHysiX 0:8c28fac22d27 204
pHysiX 0:8c28fac22d27 205 return dmpReady;
pHysiX 0:8c28fac22d27 206 }
pHysiX 0:8c28fac22d27 207
pHysiX 22:ef8aa9728013 208 // ****************************************************************
pHysiX 22:ef8aa9728013 209 // === MPL3115A2 SETUP ROUTINE ===
pHysiX 22:ef8aa9728013 210 // ****************************************************************
pHysiX 22:ef8aa9728013 211 bool setup_altimeter(void)
pHysiX 22:ef8aa9728013 212 {
pHysiX 22:ef8aa9728013 213 if (altimeter.init())
pHysiX 22:ef8aa9728013 214 return true;
pHysiX 22:ef8aa9728013 215 else
pHysiX 22:ef8aa9728013 216 return false;
pHysiX 22:ef8aa9728013 217 }
pHysiX 22:ef8aa9728013 218
pHysiX 12:953d25061417 219 #ifdef ENABLE_COMPASS
pHysiX 12:953d25061417 220 // ****************************************************************
pHysiX 12:953d25061417 221 // === HMC5883L SETUP ROUTINE ===
pHysiX 12:953d25061417 222 // ****************************************************************
pHysiX 12:953d25061417 223 bool setup_compass(void)
pHysiX 12:953d25061417 224 {
pHysiX 12:953d25061417 225 compass.setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_75);
pHysiX 12:953d25061417 226 return true;
pHysiX 12:953d25061417 227 }
pHysiX 12:953d25061417 228 #endif