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@20:b193a50a2ba3, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 02:55:24 2014 +0000
Revision:
20:b193a50a2ba3
Parent:
16:9072cd6fa8d1
Child:
21:b642c18eccd1
Added in ability to control PID; fixed PID initialisation bug

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