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@38:ef65533cca32, 2014-05-13 (annotated)

Committer:
pHysiX
Date:
Tue May 13 13:05:03 2014 +0000
Revision:
38:ef65533cca32
Parent:
35:6aada2e9ffb8
Child:
39:02782ad251db
Quadcopter unstable again. ; Working on KP and KI for Rate and Attitude.; Still experimenting with frequency of Rate and Attitude loop.

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