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

Committer:
pHysiX
Date:
Thu May 08 09:39:12 2014 +0000
Revision:
21:b642c18eccd1
Parent:
20:b193a50a2ba3
Child:
22:ef8aa9728013
Primary PID disabled. Secondary (rate) PID enabled. P gain tuned

Who changed what in which revision?

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