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@2:ab967d7b4346, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 11:43:32 2014 +0000
Revision:
2:ab967d7b4346
Parent:
1:43f8ac7ca6d7
Child:
3:605fbcb54e75
Added in simple ESC initialise

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 0:8c28fac22d27 4 Serial BT(p13, p14);
pHysiX 0:8c28fac22d27 5 DigitalOut BT_CMD(p12);
pHysiX 0:8c28fac22d27 6
pHysiX 0:8c28fac22d27 7 MPU6050 imu(p9, p10);
pHysiX 0:8c28fac22d27 8
pHysiX 2:ab967d7b4346 9 DigitalOut LED[] = {LED1, LED2, LED3, LED4};
pHysiX 2:ab967d7b4346 10
pHysiX 2:ab967d7b4346 11 PwmOut ESC1(p21);
pHysiX 2:ab967d7b4346 12 PwmOut ESC2(p22);
pHysiX 2:ab967d7b4346 13 PwmOut ESC3(p23);
pHysiX 2:ab967d7b4346 14 PwmOut ESC4(p24);
pHysiX 0:8c28fac22d27 15
pHysiX 0:8c28fac22d27 16 bool imu_available = false;
pHysiX 0:8c28fac22d27 17
pHysiX 0:8c28fac22d27 18 // MPU control/status vars
pHysiX 0:8c28fac22d27 19 bool dmpReady = false; // set true if DMP init was successful
pHysiX 0:8c28fac22d27 20 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
pHysiX 0:8c28fac22d27 21 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
pHysiX 0:8c28fac22d27 22
pHysiX 2:ab967d7b4346 23 bool setup_ESC(void)
pHysiX 2:ab967d7b4346 24 {
pHysiX 2:ab967d7b4346 25 ESC1.period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 26 ESC2.period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 27 ESC3.period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 28 ESC4.period_us(ESC_PERIOD_US);
pHysiX 2:ab967d7b4346 29
pHysiX 2:ab967d7b4346 30 ESC1.pulsewidth_us(0);
pHysiX 2:ab967d7b4346 31 ESC2.pulsewidth_us(0);
pHysiX 2:ab967d7b4346 32 ESC3.pulsewidth_us(0);
pHysiX 2:ab967d7b4346 33 ESC3.pulsewidth_us(0);
pHysiX 2:ab967d7b4346 34
pHysiX 2:ab967d7b4346 35 return true;
pHysiX 2:ab967d7b4346 36 }
pHysiX 2:ab967d7b4346 37
pHysiX 0:8c28fac22d27 38 bool setup_led(void)
pHysiX 0:8c28fac22d27 39 {
pHysiX 0:8c28fac22d27 40 ledReset();
pHysiX 0:8c28fac22d27 41 return true;
pHysiX 0:8c28fac22d27 42 }
pHysiX 0:8c28fac22d27 43
pHysiX 0:8c28fac22d27 44 bool setup_bt(void)
pHysiX 0:8c28fac22d27 45 {
pHysiX 0:8c28fac22d27 46 BT.baud(115200);
pHysiX 0:8c28fac22d27 47 BT_CMD = 0;
pHysiX 0:8c28fac22d27 48 BT.printf("Bluetooth online!\n");
pHysiX 0:8c28fac22d27 49 return true;
pHysiX 0:8c28fac22d27 50 }
pHysiX 0:8c28fac22d27 51
pHysiX 0:8c28fac22d27 52 bool setup_mpu6050(void)
pHysiX 0:8c28fac22d27 53 {
pHysiX 0:8c28fac22d27 54 imu.reset();
pHysiX 0:8c28fac22d27 55 imu.debugSerial.baud(115200);
pHysiX 0:8c28fac22d27 56 wait_ms(5);
pHysiX 0:8c28fac22d27 57
pHysiX 0:8c28fac22d27 58 imu.initialize();
pHysiX 0:8c28fac22d27 59 imu_available = imu.testConnection();
pHysiX 0:8c28fac22d27 60
pHysiX 0:8c28fac22d27 61 imu.debugSerial.printf("IMU status...\t\t\t");
pHysiX 0:8c28fac22d27 62 imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
pHysiX 0:8c28fac22d27 63
pHysiX 0:8c28fac22d27 64 if (imu_available) {
pHysiX 0:8c28fac22d27 65 devStatus = imu.dmpInitialize();
pHysiX 0:8c28fac22d27 66
pHysiX 0:8c28fac22d27 67 /*
pHysiX 0:8c28fac22d27 68 // supply your own gyro offsets here, scaled for min sensitivity
pHysiX 0:8c28fac22d27 69 mpu.setXGyroOffset(-61);
pHysiX 0:8c28fac22d27 70 mpu.setYGyroOffset(-127);
pHysiX 0:8c28fac22d27 71 mpu.setZGyroOffset(19);
pHysiX 0:8c28fac22d27 72 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
pHysiX 0:8c28fac22d27 73 */
pHysiX 0:8c28fac22d27 74
pHysiX 0:8c28fac22d27 75 if(!devStatus) {
pHysiX 0:8c28fac22d27 76 imu.setDMPEnabled(true);
pHysiX 0:8c28fac22d27 77 while (!imu.getIntDataReadyStatus());
pHysiX 0:8c28fac22d27 78 packetSize = imu.dmpGetFIFOPacketSize();
pHysiX 0:8c28fac22d27 79 dmpReady = true;
pHysiX 0:8c28fac22d27 80 } else {
pHysiX 0:8c28fac22d27 81 imu.debugSerial.printf("\tDMP setup failed!\n");
pHysiX 0:8c28fac22d27 82 return false;
pHysiX 0:8c28fac22d27 83 }
pHysiX 0:8c28fac22d27 84
pHysiX 0:8c28fac22d27 85 imu.resetFIFO();
pHysiX 0:8c28fac22d27 86 } else {
pHysiX 0:8c28fac22d27 87 return false;
pHysiX 0:8c28fac22d27 88 }
pHysiX 0:8c28fac22d27 89
pHysiX 0:8c28fac22d27 90 imu.debugSerial.printf("IMU setup routine done!");
pHysiX 0:8c28fac22d27 91
pHysiX 0:8c28fac22d27 92 return dmpReady;
pHysiX 0:8c28fac22d27 93 }
pHysiX 0:8c28fac22d27 94