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@3:605fbcb54e75, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 14:53:32 2014 +0000
Revision:
3:605fbcb54e75
Parent:
2:ab967d7b4346
Child:
4:01921a136f58
Fully implemented system. Need to test stability of RTOS, and to make sure that values are correct. ; Rate Mode only.

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 3:605fbcb54e75 4 #define KP_YAW_RATE 1.0
pHysiX 3:605fbcb54e75 5 #define KP_PITCH_RATE 1.0
pHysiX 3:605fbcb54e75 6 #define KP_ROLL_RATE 1.0
pHysiX 3:605fbcb54e75 7
pHysiX 0:8c28fac22d27 8 Serial BT(p13, p14);
pHysiX 0:8c28fac22d27 9 DigitalOut BT_CMD(p12);
pHysiX 0:8c28fac22d27 10
pHysiX 0:8c28fac22d27 11 MPU6050 imu(p9, p10);
pHysiX 0:8c28fac22d27 12
pHysiX 3:605fbcb54e75 13 //DigitalOut LED[4] = {LED1, LED2, LED3, LED4};
pHysiX 3:605fbcb54e75 14
pHysiX 3:605fbcb54e75 15 //Kc, Ti, Td, interval
pHysiX 3:605fbcb54e75 16 PID yawPIDrate(KP_YAW_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 17 PID pitchPIDrate(KP_PITCH_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 18 PID rollPIDrate(KP_ROLL_RATE, 0.0, 0.0, TASK2_PERIOD);
pHysiX 3:605fbcb54e75 19
pHysiX 3:605fbcb54e75 20 PwmOut ESC[4] = {p21, p22, p23, p24};
pHysiX 3:605fbcb54e75 21
pHysiX 3:605fbcb54e75 22
pHysiX 2:ab967d7b4346 23
pHysiX 3:605fbcb54e75 24 // ================================================================
pHysiX 3:605fbcb54e75 25 // === MAIN SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 26 // ================================================================
pHysiX 3:605fbcb54e75 27 bool setupALLdevices(void)
pHysiX 3:605fbcb54e75 28 {
pHysiX 3:605fbcb54e75 29 bool error = false;
pHysiX 3:605fbcb54e75 30
pHysiX 3:605fbcb54e75 31 if (!setup_ESC()) {
pHysiX 3:605fbcb54e75 32 imu.debugSerial.printf("ESC FAILED!!!\n");
pHysiX 3:605fbcb54e75 33 error = true;
pHysiX 3:605fbcb54e75 34 }
pHysiX 0:8c28fac22d27 35
pHysiX 3:605fbcb54e75 36 /*
pHysiX 3:605fbcb54e75 37 if (!setup_led())
pHysiX 3:605fbcb54e75 38 error = true;
pHysiX 3:605fbcb54e75 39 */
pHysiX 3:605fbcb54e75 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 3:605fbcb54e75 53 return error;
pHysiX 3:605fbcb54e75 54 }
pHysiX 0:8c28fac22d27 55
pHysiX 3:605fbcb54e75 56
pHysiX 3:605fbcb54e75 57
pHysiX 3:605fbcb54e75 58
pHysiX 3:605fbcb54e75 59 // ****************************************************************
pHysiX 3:605fbcb54e75 60 // === ESC SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 61 // ****************************************************************
pHysiX 2:ab967d7b4346 62 bool setup_ESC(void)
pHysiX 2:ab967d7b4346 63 {
pHysiX 3:605fbcb54e75 64 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 65 ESC[i].period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 66
pHysiX 3:605fbcb54e75 67 for (int i = 0; i < 4; i++)
pHysiX 3:605fbcb54e75 68 ESC[i].pulsewidth_us(0);
pHysiX 3:605fbcb54e75 69
pHysiX 2:ab967d7b4346 70 return true;
pHysiX 2:ab967d7b4346 71 }
pHysiX 2:ab967d7b4346 72
pHysiX 3:605fbcb54e75 73 // ****************************************************************
pHysiX 3:605fbcb54e75 74 // === BLUETOOTH SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 75 // ****************************************************************
pHysiX 0:8c28fac22d27 76 bool setup_bt(void)
pHysiX 0:8c28fac22d27 77 {
pHysiX 0:8c28fac22d27 78 BT.baud(115200);
pHysiX 0:8c28fac22d27 79 BT_CMD = 0;
pHysiX 0:8c28fac22d27 80 BT.printf("Bluetooth online!\n");
pHysiX 0:8c28fac22d27 81 return true;
pHysiX 0:8c28fac22d27 82 }
pHysiX 0:8c28fac22d27 83
pHysiX 3:605fbcb54e75 84 // ****************************************************************
pHysiX 3:605fbcb54e75 85 // === PID SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 86 // ****************************************************************
pHysiX 3:605fbcb54e75 87 bool setup_PID(void)
pHysiX 3:605fbcb54e75 88 {
pHysiX 3:605fbcb54e75 89 yawPIDrate.setInputLimits(-2720, 2720);
pHysiX 3:605fbcb54e75 90 yawPIDrate.setOutputLimits(-500, 500);
pHysiX 3:605fbcb54e75 91 yawPIDrate.setMode(AUTO_MODE);
pHysiX 3:605fbcb54e75 92
pHysiX 3:605fbcb54e75 93 pitchPIDrate.setInputLimits(-1720, 1720);
pHysiX 3:605fbcb54e75 94 pitchPIDrate.setOutputLimits(-500, 500);
pHysiX 3:605fbcb54e75 95 pitchPIDrate.setMode(AUTO_MODE);
pHysiX 3:605fbcb54e75 96
pHysiX 3:605fbcb54e75 97 rollPIDrate.setInputLimits(-2720, 2720);
pHysiX 3:605fbcb54e75 98 rollPIDrate.setOutputLimits(-500, 500);
pHysiX 3:605fbcb54e75 99 rollPIDrate.setMode(AUTO_MODE);
pHysiX 3:605fbcb54e75 100 return true;
pHysiX 3:605fbcb54e75 101 }
pHysiX 3:605fbcb54e75 102
pHysiX 3:605fbcb54e75 103 // ****************************************************************
pHysiX 3:605fbcb54e75 104 // === MPU6050 SETUP ROUTINE ===
pHysiX 3:605fbcb54e75 105 // ****************************************************************
pHysiX 3:605fbcb54e75 106 bool imu_available = false;
pHysiX 3:605fbcb54e75 107
pHysiX 3:605fbcb54e75 108 /*/ MPU control/status variables: */
pHysiX 3:605fbcb54e75 109 bool dmpReady = false; // set true if DMP init was successful
pHysiX 3:605fbcb54e75 110 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
pHysiX 3:605fbcb54e75 111 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
pHysiX 3:605fbcb54e75 112
pHysiX 0:8c28fac22d27 113 bool setup_mpu6050(void)
pHysiX 0:8c28fac22d27 114 {
pHysiX 0:8c28fac22d27 115 imu.reset();
pHysiX 0:8c28fac22d27 116 imu.debugSerial.baud(115200);
pHysiX 0:8c28fac22d27 117 wait_ms(5);
pHysiX 0:8c28fac22d27 118
pHysiX 0:8c28fac22d27 119 imu.initialize();
pHysiX 0:8c28fac22d27 120 imu_available = imu.testConnection();
pHysiX 0:8c28fac22d27 121
pHysiX 0:8c28fac22d27 122 imu.debugSerial.printf("IMU status...\t\t\t");
pHysiX 0:8c28fac22d27 123 imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
pHysiX 0:8c28fac22d27 124
pHysiX 0:8c28fac22d27 125 if (imu_available) {
pHysiX 0:8c28fac22d27 126 devStatus = imu.dmpInitialize();
pHysiX 0:8c28fac22d27 127
pHysiX 3:605fbcb54e75 128 // supply your own gyro offsets here, scaled for min sensitivity
pHysiX 3:605fbcb54e75 129 imu.setXGyroOffset(55);
pHysiX 3:605fbcb54e75 130 imu.setYGyroOffset(3);
pHysiX 3:605fbcb54e75 131 imu.setZGyroOffset(-2);
pHysiX 3:605fbcb54e75 132 //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
pHysiX 0:8c28fac22d27 133
pHysiX 0:8c28fac22d27 134 if(!devStatus) {
pHysiX 0:8c28fac22d27 135 imu.setDMPEnabled(true);
pHysiX 0:8c28fac22d27 136 while (!imu.getIntDataReadyStatus());
pHysiX 0:8c28fac22d27 137 packetSize = imu.dmpGetFIFOPacketSize();
pHysiX 0:8c28fac22d27 138 dmpReady = true;
pHysiX 0:8c28fac22d27 139 } else {
pHysiX 0:8c28fac22d27 140 imu.debugSerial.printf("\tDMP setup failed!\n");
pHysiX 0:8c28fac22d27 141 return false;
pHysiX 0:8c28fac22d27 142 }
pHysiX 0:8c28fac22d27 143
pHysiX 0:8c28fac22d27 144 imu.resetFIFO();
pHysiX 0:8c28fac22d27 145 } else {
pHysiX 0:8c28fac22d27 146 return false;
pHysiX 0:8c28fac22d27 147 }
pHysiX 0:8c28fac22d27 148
pHysiX 0:8c28fac22d27 149 imu.debugSerial.printf("IMU setup routine done!");
pHysiX 0:8c28fac22d27 150
pHysiX 0:8c28fac22d27 151 return dmpReady;
pHysiX 0:8c28fac22d27 152 }
pHysiX 0:8c28fac22d27 153