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@12:953d25061417, 2014-05-02 (annotated)

Committer:
pHysiX
Date:
Fri May 02 17:01:56 2014 +0000
Revision:
12:953d25061417
Parent:
10:ef5fe86f67fe
Child:
14:267368c83b6a
Added in all sensors. Need to add in EEPROM to complete control of Tilty. Finished all telemetry output and appropriate data rates.

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