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@48:9dbdc4144f00, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 13:21:17 2014 +0000
Revision:
48:9dbdc4144f00
Parent:
39:02782ad251db
Child:
49:c882f9135033
Altimeter and IMU changed to non-blocking I2C

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