Revision 55:fc7cc7a4821c, committed 2014-05-19

Comitter:

pHysiX

Date:

Mon May 19 16:18:33 2014 +0000

Parent:

54:a36d39a90c21

Commit message:

Restructured code. Licence reproduction and citation of sources not present when code was made public. Code now withdrawn and kept private

Changed in this revision

--- a/Libraries/HMC5883L.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/HMC5883L/#5104b90e3f0f

--- a/Libraries/I2cRtosDriver.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/I2cRtosDriver/#2c6432b37cce

--- a/Libraries/MPL3115A2.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/MPL3115A2/#fb9726aaa41d

--- a/Libraries/MPU6050-DMP-Blocking.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/MPU6050-DMP-Blocking/#1171abc68d09

--- a/Libraries/MPU6050-DMP-NonBlocking.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/MPU6050-DMP-NonBlocking/#5d1e2e63bbca

--- a/Libraries/PID.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/PID/#b55a16b5f05c

--- a/Libraries/PwmInRC.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/pHysiX/code/PwmInRC/#2a58e3363242

--- a/Libraries/mbed-rtos.lib	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed-rtos/#5dfe422a963d

--- a/Libraries/mbed.bld	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file

--- a/RTOS-Setup/config.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,35 +0,0 @@
-#ifndef _CONFIG_H_
-#define _CONFIG_H_
-
-#define STICK_GAIN 2
-#define STICK_GAIN_YAW 24
-
-// 1khz / (1 + 9) = 100 Hz (Fsample)
-#define IMU_SAMPLE_RATE_DIVIDER 9
-// Fsample Hz / (1 + 0) = 100 Hz
-#define IMU_FIFO_RATE_DIVIDER   0x00
-
-// Frequency (Hz):
-#define TASK1_FREQUENCY             50
-#define TASK2_MASTER_FREQUENCY      200
-#define TASK2_SLAVE_FREQUENCY       400
-#define TASK3_FREQUENCY             50
-#define TASK4_FREQUENCY             400
-
-#define ESC_FREQUENCY               400
-
-#define P_Y_RATE                    8.0               
-#define P_P_RATE                    6.5 
-#define P_R_RATE                    6.5 
-
-#define TI_Y_RATE                   2.0 
-#define TI_P_RATE                   2.0     // Maybe 2.5-4.0
-#define TI_R_RATE                   2.0     // Maybe 2.5-4.0
-
-#define P_P_ATTITUDE                0.5 
-#define P_R_ATTITUDE                0.4 
-
-#define TI_P_ATTITUDE               5.0    //5.0 // Maybe 8.5-10.0
-#define TI_R_ATTITUDE               5.0    //5.0 // Maybe 8.5-10.0
-
-#endif

--- a/RTOS-Setup/inc/setup.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,65 +0,0 @@
-/* File:    setup.h
- * Author:  Trung Tin Ian HUA
- * Date:    May 2014
- * Purpose: Setup code to initialise all devices.
- */
-#include "mbed.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "MPU6050_6Axis_MotionApps20_NB.h"
-#include "PID.h"
-#include "MPL3115A2.h"
-
-#include "config.h"
-
-#ifdef ENABLE_COMPASS
-#include "HMC5883L.h"
-#endif
-
-#ifndef _SETUP_H_
-#define _SETUP_H_
-
-/* PID Gains: */
-extern float KP_YAW_RATE;
-extern float KP_PITCH_RATE;
-extern float KP_ROLL_RATE;
-extern float TI_YAW_RATE;
-extern float TI_PITCH_RATE;
-extern float TI_ROLL_RATE;
-
-extern float KP_PITCH_ATTITUDE;
-extern float KP_ROLL_ATTITUDE;
-extern float TI_PITCH_ATTITUDE;
-extern float TI_ROLL_ATTITUDE;
-
-/* Sensors and devices: */
-extern Serial BT;
-extern Serial PC;
-extern DigitalOut BT_CMD;
-extern AnalogIn voltageSense;
-//extern MPU6050 imu;
-extern MPU6050_NB imu_nb;
-extern uint16_t packetSize;
-extern MPL3115A2 altimeter;
-extern PwmOut ESC[4];
-
-/* PID Devices: */
-extern PID pitchPIDattitude;
-extern PID rollPIDattitude;
-extern PID yawPIDrate;
-extern PID pitchPIDrate;
-extern PID rollPIDrate;
-
-/* Setup routines: */
-bool setupALLdevices(void);
-bool setup_ESC(void);
-bool setup_bt(void);
-bool setup_PID(void);
-bool setup_mpu6050(void);
-bool setup_altimeter(void);
-
-#ifdef ENABLE_COMPASS
-extern HMC5883L compass;
-bool setup_compass(void);
-#endif
-
-#endif

--- a/RTOS-Setup/inc/tasks.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,24 +0,0 @@
-/* File:    tasks.h
- * Author:  Trung Tin Ian HUA
- * Date:    May 2014
- * Purpose: Code to intialise and start all threads.
- */
-#include "mbed.h"
-#include "rtos.h"
-
-#ifndef _TASKS_H_
-#define _TASKS_H_
-
-#include "Task1.h"
-#include "Task2_Master.h"
-#include "Task2_Slave.h"
-#include "Task3.h"
-#include "Task4.h"
-
-#define ESC_PERIOD_US               1000000/ESC_FREQUENCY 
-
-extern Mutex mutex_i2c;
-
-void createThreads(void);
-
-#endif

--- a/RTOS-Setup/src/setup.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,239 +0,0 @@
-/* File:    Setup.h
- * Author:  Trung Tin Ian HUA
- * Date:    May 2014
- * Purpose: Setup code to initialise all device
- */
-#include "setup.h"
-#include "tasks.h"
-
-Serial BT(p28, p27);
-Serial PC(USBTX, USBRX);
-DigitalOut BT_CMD(p29);
-//MPU6050 imu(p9, p10);           
-MPU6050_NB imu_nb(p9, p10); 
-MPL3115A2 altimeter(p9, p10);
-AnalogIn voltageSense(p20);
-#ifdef ENABLE_COMPASS
-HMC5883L compass(p9, p10);
-#endif
-
-float KP_YAW_RATE           =   P_Y_RATE;
-float KP_PITCH_RATE         =   P_P_RATE;
-float KP_ROLL_RATE          =   P_R_RATE;
-
-float TI_YAW_RATE           =   TI_Y_RATE;
-float TI_PITCH_RATE         =   TI_P_RATE;
-float TI_ROLL_RATE          =   TI_R_RATE;
-
-float KP_PITCH_ATTITUDE     =   P_P_ATTITUDE;
-float KP_ROLL_ATTITUDE      =   P_R_ATTITUDE;
-
-float TI_PITCH_ATTITUDE     =   TI_P_ATTITUDE;
-float TI_ROLL_ATTITUDE      =   TI_R_ATTITUDE;
-
-PID yawPIDrate(KP_YAW_RATE, TI_Y_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
-PID pitchPIDrate(KP_PITCH_RATE, TI_PITCH_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
-PID rollPIDrate(KP_ROLL_RATE, TI_ROLL_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
-
-PID pitchPIDattitude(KP_PITCH_ATTITUDE, TI_PITCH_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);
-PID rollPIDattitude(KP_ROLL_ATTITUDE, TI_ROLL_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);
-
-PwmOut ESC[4] = {p21, p22, p23, p24};
-
-
-
-// ==========================
-// === MAIN SETUP ROUTINE ===
-// ==========================
-bool setupALLdevices(void)
-{   
-    bool error = false;
-    box_demo = false;
-    mode = ATTITUDE;
-
-    if (!setup_ESC()) {
-        PC.printf("ESC FAILED!!!\n");
-        error = true;
-    }
-
-    if (setup_bt())
-        PC.printf("BT established!\n");
-    else error = true;
-
-    if (setup_PID()) 
-        PC.printf("PID established!\n");
-    else error = true;
-
-    if (setup_mpu6050()) 
-        PC.printf("MPU6050 established!\n");
-    else error = true;
-
-    if (setup_altimeter()) 
-        PC.printf("Altimeter established!\n");
-     else {
-         error = true;
-        PC.printf("ALTIMETER FAILED\n");
-     }
-
-#ifdef ENABLE_COMPASS
-    if (setup_compass())
-        PC.printf("Compass established!\n");
-    else error = true;
-#endif
-
-    return error;
-}
-
-
-
-
-// *************************
-// *** ESC SETUP ROUTINE ***
-// *************************
-bool setup_ESC(void)
-{
-    for (int i = 0; i < 4; i++)
-        ESC[i].period_us(ESC_PERIOD_US);
-
-    for (int i = 0; i < 4; i++)
-        ESC[i].pulsewidth_us(1000);
-
-    for (int i = 0; i < 4; i++)
-        ESCpower[i] = 990;
-
-    armed = false;
-
-    return true;
-}
-
-// ****************************************************************
-// === BLUETOOTH SETUP ROUTINE ===
-// ****************************************************************
-bool setup_bt(void)
-{
-    BT.baud(115200);
-    BT_CMD = 0;     // Place bluetooth into normal mode
-    BT.printf("Bluetooth online!\n");
-    return true;
-}
-
-// ****************************************************************
-// === PID SETUP ROUTINE ===
-// ****************************************************************
-bool setup_PID(void)
-{
-    pitchPIDattitude.setInputLimits(-90.0, 90.0);
-    pitchPIDattitude.setOutputLimits(-250, 250.0);
-    pitchPIDattitude.setBias(0.0);
-    pitchPIDattitude.setMode(AUTO_MODE);
-
-    rollPIDattitude.setInputLimits(-90.0, 90.0);
-    rollPIDattitude.setOutputLimits(-250, 250.0);
-    rollPIDattitude.setBias(0.0);
-    rollPIDattitude.setMode(AUTO_MODE);
-
-    yawPIDrate.setInputLimits(-500.0, 500.0);
-    yawPIDrate.setOutputLimits(-200.0, 200.0);
-    yawPIDrate.setBias(0.0);
-    yawPIDrate.setMode(AUTO_MODE);
-
-    pitchPIDrate.setInputLimits(-500.0, 500.0);
-    pitchPIDrate.setOutputLimits(-200.0, 200.0);
-    pitchPIDrate.setBias(0.0);
-    pitchPIDrate.setMode(AUTO_MODE);
-
-    rollPIDrate.setInputLimits(-500.0, 500.0);
-    rollPIDrate.setOutputLimits(-200.0, 200.0);
-    rollPIDrate.setBias(0.0);
-    rollPIDrate.setMode(AUTO_MODE);
-
-    pitchPIDattitude.reset();
-    rollPIDattitude.reset();
-    yawPIDrate.reset();
-    pitchPIDrate.reset();
-    rollPIDrate.reset();
-
-    return true;
-}
-
-// ****************************************************************
-// === MPU6050 SETUP ROUTINE ===
-// ****************************************************************
-bool imu_available = false;
-
-/*/ MPU control/status variables: */
-bool dmpReady = false;          // set true if DMP init was successful
-uint8_t devStatus;              // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;            // expected DMP packet size (default is 42 bytes)
-
-bool setup_mpu6050(void)
-{
-    MPU6050 imu(p9, p10); 
-    
-    imu.reset();
-    imu.debugSerial.baud(115200);
-    wait_ms(5);
-
-    imu.initialize();
-    imu_available = imu.testConnection();
-
-    imu.debugSerial.printf("imu status...\t\t\t");
-    imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
-
-    if (imu_available) {
-        devStatus = imu.dmpInitialize();
-        // supply your own gyro offsets here, scaled for min sensitivity
-        //imu.setXGyroOffset(55);
-        //imu.setYGyroOffset(3);
-        //imu.setZGyroOffset(-2);
-        //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
-
-        if(!devStatus) {
-            imu.setDMPEnabled(true);
-            while (!imu.getIntDataReadyStatus());
-            packetSize = imu.dmpGetFIFOPacketSize();
-            imu.debugSerial.printf("Packet Size: %d\n", packetSize);
-            dmpReady = true;
-
-            int8_t xgOffsetTC = imu.getXGyroOffset();
-            int8_t ygOffsetTC = imu.getYGyroOffset();
-            int8_t zgOffsetTC = imu.getZGyroOffset();
-
-            imu.debugSerial.printf("X Offset: %4d Y Offset: %4d Z Offset: %4d\n", xgOffsetTC, ygOffsetTC, zgOffsetTC);
-
-        } else {
-            imu.debugSerial.printf("\tDMP setup failed!\n");
-            return false;
-        }
-
-        imu.resetFIFO();
-    } else {
-        return false;
-    }
-
-    imu.debugSerial.printf("imu setup routine done!");
-
-    return dmpReady;
-}
-
-// ****************************************************************
-// === MPL3115A2 SETUP ROUTINE ===
-// ****************************************************************
-bool setup_altimeter(void)
-{
-    if (altimeter.init())
-        return true;
-    else
-        return false;
-}
-
-#ifdef ENABLE_COMPASS
-// ****************************************************************
-// === HMC5883L SETUP ROUTINE ===
-// ****************************************************************
-bool setup_compass(void)
-{
-    compass.setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_75);
-    return true;
-}
-#endif

--- a/RTOS-Setup/src/tasks.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-/* File:    tasks.h
- * Author:  Trung Tin Ian HUA
- * Date:    May 2014
- * Purpose: Code to intialise and start all threads.
- */
-#include "tasks.h"
-#include "setup.h"
-
-Mutex mutex_i2c;
-
-void createThreads(void)
-{
-    /* Create timer callbacks */
-    RtosTimer thread2Slave_ISR(Task2_Slave_ISR, osTimerPeriodic, NULL);
-    thread2Slave_ISR.start(TASK2_SLAVE_PERIOD);
-    
-    RtosTimer thread2Master_ISR(Task2_Master_ISR, osTimerPeriodic, NULL);
-    thread2Master_ISR.start(TASK2_MASTER_PERIOD);
-    
-    RtosTimer thread3(Task3, osTimerPeriodic, NULL);
-    thread3.start(TASK3_PERIOD);
-    
-    //RtosTimer thread4(Task4, osTimerPeriodic, NULL);
-    //thread4.start(TASK4_PERIOD);
-    
-    /* Create threads */
-    Thread thread1(Task1, NULL, osPriorityIdle);
-    Thread thread2Slave(Task2_Slave, NULL, osPriorityRealtime);
-    Thread thread2Master(Task2_Master, NULL, osPriorityHigh);
-    Thread thread4(Task4, NULL, osPriorityRealtime);
-
-    /* Execute state machine forever */
-    Thread::wait(osWaitForever);
-}

--- a/RTOS-Threads/inc/Task1.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,32 +0,0 @@
-/* File:        Task1.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread1: Telemetry output
- * Settings:    100Hz
- */ 
-#include "mbed.h"
-#include "rtos.h"
-#include "tasks.h"
-
-#ifndef _TASK1_H_
-#define _TASK1_H_
-
-// ms timing: Refer to tasks.h to change frequency of Task1.
-#define TASK1_PERIOD            1000/TASK1_FREQUENCY
-
-/* Scale:
-Vin = ADC * Vread * scale
-Vin = (2^n-1)/3.3 * k*Vin * scale * ADJUST
-*/
-#define VOLTAGE_SCALE           49.60248447
-
-extern bool box_demo;
-extern bool rc_out;
-extern bool gyro_out;
-extern bool command_check;
-extern bool adjust_check;
-
-/* Thread1: Telemetry output */
-void Task1(void const *argument);
-
-#endif

--- a/RTOS-Threads/inc/Task2_Master.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-/* File:        Task2_Master.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread2M: Master PID control loop (attitude)
- * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
- * Settings:    200Hz
- */
-#include "mbed.h"
-#include "rtos.h"
-#include "tasks.h"
-
-#ifndef _TASK2_MASTER_H_
-#define _TASK2_MASTER_H_
-
-// ms timing: Refer to tasks.h to change frequency of Task2_Master.
-#define TASK2_MASTER_PERIOD             1000/TASK2_MASTER_FREQUENCY
-
-
-#ifdef ENABLE_COMPASS
-extern double heading;
-#endif
-
-extern volatile float ypr[3];
-extern volatile float adjust_attitude[3];
-
-extern float altitude, temperature;
-
-/* Thread2-Master: Attitude PID Control */
-void Task2_Master(void const *argument);
-
-// ========================
-// === Helper functions ===
-// ========================
-void Task2_Master_ISR(void const *argument);
-void AHRSSample(void);
-
-#endif

--- a/RTOS-Threads/inc/Task2_Slave.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,33 +0,0 @@
-/* File:        Task2_Slave.cpp
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread2S: Slave PID control loop (rate)
- * Functions:   Gyro sample
- * Settings:    400Hz
- */
-#include "mbed.h"
-#include "rtos.h"
-#include "tasks.h"
-
-#ifndef _TASK2_SLAVE_H_
-#define _TASK2_SLAVE_H_
-
-// ms timing: Refer to tasks.h to change frequency of Task2_Slave.
-#define TASK2_SLAVE_PERIOD 1000/TASK2_MASTER_FREQUENCY
-
-extern volatile float adjust[3];
-extern int16_t gz;
-extern volatile int gyro[3];
-
-extern Semaphore sem_Task2_Slave;
-
-/* Thread2-Slave: Gyro sample and Rate PID Control */
-void Task2_Slave(void const *argument);
-
-// ========================
-// === Helper functions ===
-// ========================
-void Task2_Slave_ISR(void const *argument);
-void gyroSample(void);
-
-#endif

--- a/RTOS-Threads/inc/Task3.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,40 +0,0 @@
-/* File:        Task3.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread3: RC & BT Command, and Telemetry
- * Settings:    50Hz
- */ 
-#include "mbed.h"
-#include "rtos.h"
-#include "tasks.h"
-
-#ifndef _TASK3_H_
-#define _TASKS3_H_
-
-// ms timing: Refer to tasks.h to change frequency of Task3.
-#define TASK3_PERIOD 1000/TASK3_FREQUENCY
-
-enum FLIGHT_MODE {
-    RATE,
-    ATTITUDE
-};
-
-extern volatile int RCCommand[5];
-extern volatile int inputYPR[3];
-extern volatile float ypr_offset[3];
-
-extern FLIGHT_MODE mode;
-
-/* Thread3: RC & BT Command, and Telemetry */
-void Task3(void const *argument);
-
-// ========================
-// === Helper functions ===
-// ========================
-int constrainRCCommand(int input);
-int deadbandInputYPR(int input);
-int constrainInputY(int input);
-int constrainInputPR(int input);
-void uartDecoder(char input);
-
-#endif

--- a/RTOS-Threads/inc/Task4.h	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,35 +0,0 @@
-/* File:        Task4.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread4: ESC pulsewidth update. Note this is INDEPENDENT of the pulse frequency.
- * Settings:    200Hz
- * 200Hz <= PWM frequency <= 400Hz
- * Refer to tasks.h to change PWM frequency 
- */ 
-#include "mbed.h"
-#include "rtos.h"
-#include "tasks.h"
-
-#ifndef _TASK4_H_
-#define _TASK4_H_
-
-// ms timing: Refer to tasks.h to change frequency of Task4.
-#define TASK4_PERIOD 1000/TASK4_FREQUENCY
-
-extern volatile int ESCpower[4];
-
-extern bool armed;
-extern bool ESC_check;
-extern bool calibration_mode;
-
-extern Semaphore sem_Task4;
-
-/* Thread4: Update ESC pulsewidth */
-void Task4(void const *argument);
-
-// ========================
-// === Helper functions ===
-// ========================
-int constrainESC(float input);
-
-#endif

--- a/RTOS-Threads/src/Task1.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,62 +0,0 @@
-/* File:        Task1.cpp
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Functions:   Thread1: Telemetry output
- * Settings:    100Hz
- * Timing:
- */
-#include "Task1.h"
-#include "setup.h"
-
-bool box_demo = false;
-bool rc_out = false;
-bool gyro_out = false;
-bool command_check = false;
-bool adjust_check = false;
-int voltageUpdate = 0;
-
-float vIn = 0.0;
-
-
-
-// =================
-// === TELEMETRY ===
-// =================
-void Task1(void const *argument)
-{
-    while(1) {
-        if (box_demo) {
-            BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0], ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
-            BT.printf("\nA%3.2f\nT%2.2f\n", altitude, temperature);
-            
-            if (voltageUpdate > 3) {
-                BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
-                voltageUpdate = 0;
-            } else {
-                voltageUpdate++;
-            }
-        }
-
-        else if (gyro_out)
-            BT.printf("%3d %3d %3d\n", (int)gyro[0], (int)gyro[1], (int)gyro[2]);
-        //BT.printf("%3d %3d %3d\n", (int)gyro[0], (int)gyro[1], gz);
-
-        else if (rc_out)
-            BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
-
-        else if (command_check)
-            BT.printf("%3d %3d %3d\n", inputYPR[0], inputYPR[1], inputYPR[2]);
-
-        else if (adjust_check)
-            BT.printf("%3.2f %3.2f %3.2f\n", adjust[0], adjust[1], adjust[2]);
-
-        else if (ESC_check)
-            BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
-
-        else {}
-
-        //PC.printf("T1 End\n");
-        Thread::wait(TASK1_PERIOD);
-    }
-}
-//Timer

--- a/RTOS-Threads/src/Task2_Master.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,137 +0,0 @@
-/* File:        Task2_Master.cpp
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread2M: Master PID control loop (attitude)
- * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
- * Settings:    200Hz
- * Timing:
- */
-#include "Task2_Slave.h"
-#include "setup.h"
-#include "PID.h"
-
-Semaphore sem_Task2_Master(1);
-
-/* MPU6050 control/status variables: */
-uint8_t mpuIntStatus;       // holds actual interrupt status byte from MPU
-uint16_t fifoCount;         // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64];     // FIFO storage buffer
-
-/* Orientation/motion variables: */
-Quaternion q;               // [w, x, y, z] quaternion container
-VectorFloat gravity;        // [x, y, z] gravity vector
-float ypr_rad[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
-volatile float ypr[3];
-
-#ifndef M_PI
-#define M_PI 3.1415
-#endif
-
-#ifdef ENABLE_COMPASS
-//int compassX, compassY, compassZ;
-double heading = 0;
-#endif
-
-float altitude, temperature;
-
-/* YPR Adjust */
-volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
-
-
-
-
-// ===============================
-// === YPR SAMPLE & MASTER PID ===
-// ===============================
-void Task2_Master(void const *argument)
-{
-    while(1) {
-        //PC.printf("T2M\n");
-        sem_Task2_Master.wait();
-        //PC.printf("T2M Sem\n");
-        
-        mutex_i2c.lock();
-        AHRSSample();
-        altitude = altimeter.Altitude_m();
-        altimeter.Temp_C();
-        mutex_i2c.unlock();
-
-        if (armed) {
-            switch (mode) {
-                case RATE:
-                    break;
-
-                case ATTITUDE:
-                default:
-                    pitchPIDattitude.setProcessValue((int) (ypr[1] - ypr_offset[1]));
-                    rollPIDattitude.setProcessValue((int) (ypr[2] - ypr_offset[2]));
-
-                    pitchPIDattitude.setSetPoint(inputYPR[1]);
-                    rollPIDattitude.setSetPoint(inputYPR[2]);
-
-                    adjust_attitude[1] = pitchPIDattitude.compute();
-                    adjust_attitude[2] = rollPIDattitude.compute();
-                    adjust_attitude[2] *= -1;
-
-                    sem_Task2_Slave.release();
-                    break;
-            }
-        }
-        Thread::wait(TASK2_MASTER_PERIOD);
-    }
-}
-
-
-
-
-// ************************
-// *** Helper functions ***
-// ************************
-void Task2_Master_ISR(void const *argument)
-{
-    //PC.printf("T2M ISR\n");
-    sem_Task2_Master.release();
-}
-
-void AHRSSample(void)
-{
-//Timer
-    // reset interrupt flag and get INT_STATUS byte
-    mpuIntStatus = imu_nb.getIntStatus();
-
-    // get current FIFO count
-    fifoCount = imu_nb.getFIFOCount();
-    //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
-
-    // check for overflow
-    // Only keep a max of 2 packets in buffer.
-    if ((mpuIntStatus & 0x10) || fifoCount > 84) {
-        // reset so we can continue cleanly
-        imu_nb.resetFIFO();
-        //imu.debugSerial.printf("FIFO overflow!");
-        //BT.printf("FIFO overflow!\n");
-
-        // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
-        // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = imu_nb.getFIFOCount();
-
-        while (fifoCount > 41) {
-            // read a packet from FIFO
-            imu_nb.getFIFOBytes(fifoBuffer, packetSize);
-
-            // track FIFO count here in case there is > 1 packet available
-            // (this lets us immediately read more without waiting for an interrupt)
-            fifoCount -= packetSize;
-        }
-
-        // display YPR angles in degrees
-        imu_nb.dmpGetQuaternion(&q, fifoBuffer);
-        imu_nb.dmpGetGravity(&gravity, &q);
-        imu_nb.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
-
-        for (int i = 0; i < 3; i++)
-            ypr[i] = ypr_rad[i] * 180/M_PI;
-    }
-//Timer
-}

--- a/RTOS-Threads/src/Task2_Slave.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,98 +0,0 @@
-/* File:        Task2_Slave.cpp
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread2S: Slave PID control loop (rate)
- * Functions:   Gyro sample
- * Settings:    400Hz
- * Timing:      290us
- */
-#include "Task2_Slave.h"
-#include "setup.h"
-#include "PID.h"
-
-Semaphore sem_Task2_Slave(1);
-
-/* YPR Adjust */
-volatile float adjust[3] = {0.0, 0.0, 0.0};
-
-int16_t gx, gy, gz;
-volatile int gyro[3] = {0, 0, 0};
-
-
-
-
-// ===============================
-// === GYRO SAMPLE & SLAVE PID ===
-// ===============================
-void Task2_Slave(void const *argument)
-{
-    while (1) {
-        //PC.printf("T2S\n");
-        sem_Task2_Slave.wait();
-        //PC.printf("T2S Sem\n");
-        
-        mutex_i2c.lock();
-        gyroSample();
-        mutex_i2c.unlock();
-
-        if (armed) {
-            yawPIDrate.setSetPoint(inputYPR[0]);
-
-            switch (mode) {
-                case RATE:
-                    pitchPIDrate.setSetPoint(inputYPR[1]);
-                    rollPIDrate.setSetPoint(inputYPR[2]);
-
-                    yawPIDrate.setProcessValue(gyro[2]);
-                    pitchPIDrate.setProcessValue(gyro[1]);
-                    rollPIDrate.setProcessValue(gyro[0]);
-
-                    adjust[0] = yawPIDrate.compute();
-                    adjust[1] = pitchPIDrate.compute();
-                    adjust[2] = rollPIDrate.compute();
-                    
-                    break;
-
-                case ATTITUDE:
-                default:
-                    pitchPIDrate.setSetPoint(adjust_attitude[1]);
-                    rollPIDrate.setSetPoint(adjust_attitude[2]);
-
-                    yawPIDrate.setProcessValue(gyro[2]);
-                    pitchPIDrate.setProcessValue(gyro[1]);
-                    rollPIDrate.setProcessValue(gyro[0]);
-
-                    adjust[0] = yawPIDrate.compute();
-                    adjust[1] = pitchPIDrate.compute();
-                    adjust[2] = rollPIDrate.compute();
-
-                    break;
-            }
-            sem_Task4.release();
-        }
-        Thread::wait(TASK2_SLAVE_PERIOD);
-    }
-}
-
-
-
-
-// ************************
-// *** Helper functions ***
-// ************************
-void Task2_Slave_ISR(void const *argument)
-{
-    //PC.printf("T2S ISR\n");
-    sem_Task2_Slave.release();
-}
-
-void gyroSample(void)
-{
-    imu_nb.getRotation(&gx, &gy, &gz);
-    gyro[0] = gx + 60;
-    gyro[1] = gy - 15;
-    gyro[2] = gz - 8;
-
-    for (int i = 0; i < 3; i++)
-        gyro[i] /= (float) 32.8;
-}

--- a/RTOS-Threads/src/Task3.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,435 +0,0 @@
-/* File:        Task3.cpp
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread3: RC & BT Command
- * Settings:    50Hz
- */
-#include "tasks.h"
-#include "setup.h"
-#include "PwmIn.h"
-
-PwmIn rxModule[] = {p14, p15, p16, p17, p18};
-
-/* [YAW PITCH ROLL THROTTLE AUX] */
-volatile int RCCommand[5] = {0, 0, 0, 0, 0};
-/* Decoded input: [YAW PITCH ROLL] */
-volatile int inputYPR[3];
-volatile float ypr_offset[3];
-
-FLIGHT_MODE mode = ATTITUDE;
-
-
-
-
-// ======================================
-// === BT & RC COMMAND, AND TELEMETRY ===
-// ======================================
-void Task3(void const *argument)
-{
-    if (BT.readable()) {
-        char data = BT.getc();
-        uartDecoder(data);
-    }
-
-    /* Receiver decoder: */
-    RCCommand[2] = rxModule[0].pulsewidth(); // Roll
-    RCCommand[1] = rxModule[1].pulsewidth(); // Pitch
-    RCCommand[3] = rxModule[2].pulsewidth(); // Throttle
-    RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
-    RCCommand[4] = rxModule[4].pulsewidth(); // AUX
-
-    /* Lost signal (throttle): */
-    if (rxModule[2].stallTimer.read_us() > 18820) {
-        RCCommand[0] = 1500;
-        RCCommand[1] = 1500;
-        RCCommand[2] = 1500;
-        RCCommand[3] = 1000;
-        RCCommand[4] = 1000;
-    } else {
-        for (int i = 0; i < 5; i++)
-            RCCommand[i] = constrainRCCommand(RCCommand[i]);
-    }
-
-    /* Mode switching: */
-    if (RCCommand[4] > 1500) {
-        if (mode == RATE) {
-        } else if (mode == ATTITUDE) {
-            mode = RATE;
-            yawPIDrate.reset();
-        } else {}
-    } else {
-        if (mode == ATTITUDE) {
-        } else if (mode == RATE) {
-            mode = ATTITUDE;
-            yawPIDrate.reset();
-        }
-    }
-
-    /* Command decoder: */
-    inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
-    switch (mode) {
-        case RATE:
-            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
-            inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
-            break;
-        case ATTITUDE:
-        default:
-            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
-            inputYPR[2] = (RCCommand[2]-1500)*-1*9/100*STICK_GAIN;
-            break;
-    }
-
-    for (int i = 0; i < 3; i++)
-        deadbandInputYPR(inputYPR[i]);
-
-    inputYPR[0] = constrainInputY(inputYPR[0]);
-
-    for (int i = 1; i < 3; i++)
-        inputYPR[i] = constrainInputPR(inputYPR[i]);
-}
-
-
-
-
-// ************************
-// *** Helper functions ***
-// ************************
-int constrainRCCommand(int input)
-{
-    if (input < 1000)
-        return 1000;
-    else if (input > 2000)
-        return 2000;
-    else
-        return input;
-}
-
-int deadbandInputYPR(int input)
-{
-    if (input > -4 && input < 4) // if (input > -2 && input < 4)
-        return 0;
-    else
-        return input;
-}
-
-int constrainInputY(int input)
-{
-    if (input < -720)
-        return -720;
-    else if (input > 720)
-        return 720;
-    else
-        return input;
-}
-
-int constrainInputPR(int input)
-{
-    if (input < -45)
-        return -45;
-    else if (input > 45)
-        return 45;
-    else
-        return input;
-}
-
-void uartDecoder(char input)
-{
-    switch (input) {
-        case '9':
-        case '0':
-            armed = false;
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-
-            pitchPIDattitude.reset();
-            rollPIDattitude.reset();
-            yawPIDrate.reset();
-            pitchPIDrate.reset();
-            rollPIDrate.reset();
-
-            armed? BT.printf("ARMED\n") : BT.printf("DISARMED\n");
-            break;
-
-        case 'D':
-        case 'd':
-            armed = false;
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-
-            ypr_offset[0] = ypr[0];
-            ypr_offset[1] = ypr[1];
-            ypr_offset[2] = ypr[2];
-
-            pitchPIDattitude.reset();
-            rollPIDattitude.reset();
-            yawPIDrate.reset();
-            pitchPIDrate.reset();
-            rollPIDrate.reset();
-
-            armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
-            break;
-
-        case 'B':
-            box_demo = true;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-            break;
-
-        case 'Z':
-            ypr_offset[0] = ypr[0];
-            ypr_offset[1] = ypr[1];
-            ypr_offset[2] = ypr[2];
-            break;
-
-        case 'R':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = true;
-            calibration_mode = false;
-            adjust_check = false;
-            break;
-
-        case 'r':
-            box_demo = false;
-            rc_out = true;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-            break;
-
-        case 'G':
-        case 'g':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = true;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-            break;
-
-        case '1':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_YAW_RATE += 0.1;
-            yawPIDrate.setKP(KP_YAW_RATE);
-            BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
-            break;
-        case 'Q':
-        case 'q':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_YAW_RATE -= 0.1;
-            yawPIDrate.setKP(KP_YAW_RATE);
-            BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
-            break;
-
-        case '2':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_PITCH_RATE += 0.1;
-            pitchPIDrate.setKP(KP_PITCH_RATE);
-            BT.printf("KP P rate: %2.5f\n", KP_PITCH_RATE);
-            break;
-        case 'W':
-        case 'w':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_PITCH_RATE -= 0.1;
-            pitchPIDrate.setKP(KP_PITCH_RATE);
-            BT.printf("KP P rate: %3.4f\n", KP_PITCH_RATE);
-            break;
-
-        case '3':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_ROLL_RATE += 0.1;
-            rollPIDrate.setKP(KP_ROLL_RATE);
-            BT.printf("KP R rate: %3.4f\n", KP_ROLL_RATE);
-            break;
-        case 'E':
-        case 'e':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_ROLL_RATE -= 0.1;
-            rollPIDrate.setKP(KP_ROLL_RATE);
-            BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
-            break;
-
-        case '6':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_PITCH_ATTITUDE += 0.1;
-            pitchPIDattitude.setKP(KP_PITCH_ATTITUDE);
-            BT.printf("KP P attitude: %2.5f\n", KP_PITCH_ATTITUDE);
-            break;
-        case 'Y':
-        case 'y':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_PITCH_ATTITUDE -= 0.1;
-            pitchPIDattitude.setKP(KP_PITCH_ATTITUDE);
-            BT.printf("KP P attitude: %2.5f\n", KP_PITCH_ATTITUDE);
-            break;
-
-        case '7':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_ROLL_ATTITUDE += 0.1;
-            rollPIDattitude.setKP(KP_ROLL_ATTITUDE);
-            BT.printf("KP R attitude: %2.5f\n", KP_ROLL_ATTITUDE);
-            break;
-        case 'U':
-        case 'u':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            KP_ROLL_ATTITUDE -= 0.1;
-            rollPIDattitude.setKP(KP_ROLL_ATTITUDE);
-            BT.printf("KP R attitude: %2.5f\n", KP_ROLL_ATTITUDE);
-            break;
-
-        case 'A':
-            if (!armed) {
-                if (RCCommand[3] < 1001) {
-                    pitchPIDattitude.reset();
-                    rollPIDattitude.reset();
-                    yawPIDrate.reset();
-                    pitchPIDrate.reset();
-                    rollPIDrate.reset();
-
-                    ypr_offset[0] = ypr[0];
-                    ypr_offset[1] = ypr[1];
-                    ypr_offset[2] = ypr[2];
-
-                    armed = true;
-                } else {
-                    BT.printf("Check Throttle\n");
-                }
-            } else {
-                BT.printf("ALREADY ARMED!!!\n");
-            }
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-            armed? BT.printf("ARMED\n"): BT.printf("ARM FAIL\n");
-            break;
-        case 'a':
-            if (armed) {
-                armed = false;
-                BT.printf("DISARMED\n");
-                ypr_offset[0] = ypr[0];
-                ypr_offset[1] = ypr[1];
-                ypr_offset[2] = ypr[2];
-                pitchPIDattitude.reset();
-                rollPIDattitude.reset();
-            } else {
-                BT.printf("ALREADY DISARMED!!!\n");
-            }
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-
-            yawPIDrate.reset();
-            pitchPIDrate.reset();
-            rollPIDrate.reset();
-
-            armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
-            break;
-
-        case 'P':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = false;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = true;
-            break;
-
-        case 'p':
-            box_demo = false;
-            rc_out = false;
-            gyro_out = false;
-            ESC_check = true;
-            command_check = false;
-            calibration_mode = false;
-            adjust_check = false;
-            break;
-
-        case 'C':
-        case 'c':
-            box_demo = false;
-            rc_out = true;
-            gyro_out = false;
-            ESC_check = false;
-            calibration_mode = true;
-            command_check = false;
-            adjust_check = false;
-
-            BT.printf("Calibration mode...\n");
-            armed? BT.printf("ARMED\n") : BT.printf("ARM FAILED\n");
-            break;
-
-        case 'M':
-        case 'm':
-            switch (mode) {
-                case RATE:
-                    BT.printf("RATE MODE\n");
-                    break;
-                case ATTITUDE:
-                default:
-                    BT.printf("ATTITUDE MODE\n");
-                    break;
-            }
-            break;
-
-        default:
-            break;
-    }
-}

--- a/RTOS-Threads/src/Task4.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,89 +0,0 @@
-/* File:        Task4.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread4: ESC pulsewidth update. Note this is INDEPENDENT of the pulse frequency.
- * Settings:    400Hz
- * 200Hz <= PWM frequency <= 400Hz
- * Frequency:   400Hz
- * Refer to tasks.h to change PWM frequency
- * Timing:
- */
-#include "tasks.h"
-#include "setup.h"
-
-Semaphore sem_Task4(1);
-
-volatile int ESCpower[4] = {0, 0, 0, 0};
-int stallESC = 0;
-
-bool armed = false;
-bool ESC_check = false;
-bool calibration_mode = false;
-
-
-
-
-// ==================
-// === ESC UPDATE ===
-// ==================
-void Task4(void const *argurment)
-{
-    while (1) {
-        if (calibration_mode) {
-            if (armed)
-                for (int i = 0; i < 4; i++)
-                    ESC[i].pulsewidth_us(RCCommand[3]);
-        } else if (!armed) {
-            if (ESC_check) {
-                BT.printf("Need to ARM to check ESC output!\n");
-                ESC_check = false;
-            }
-
-            for (int i = 0; i < 4; i++) {
-                ESCpower[i] = 990;
-                ESC[i].pulsewidth_us(ESCpower[i]);
-            }
-        } else if (armed) {
-            if (RCCommand[3] < 1100) {
-                for (int i = 0; i < 4; i++) {
-                    ESCpower[i] = 1000;
-                    ESC[i].pulsewidth_us(ESCpower[i]);
-                }
-            } else {
-                //PC.printf("T4\n");
-                sem_Task4.wait();
-                //PC.printf("T4 Sem\n");
-
-                for (int i = 0; i < 3; i++)
-                    adjust[i] /= 2.0;
-
-                int throttle = RCCommand[3] * 9/10;
-
-                ESCpower[0] = constrainESC(throttle + (adjust[1]) + (adjust[2]) - adjust[0]);
-                ESCpower[1] = constrainESC(throttle + (adjust[1]) - (adjust[2]) + adjust[0]);
-                ESCpower[2] = constrainESC(throttle - (adjust[1]) - (adjust[2]) - adjust[0]);
-                ESCpower[3] = constrainESC(throttle - (adjust[1]) + (adjust[2]) + adjust[0]);
-
-                for (int i = 0; i < 4; i++)
-                    ESC[i].pulsewidth_us(ESCpower[i]);
-            } // else
-        } //else if (armed)
-        Thread::wait(TASK4_PERIOD);
-    } //while(1)
-} //Task4
-
-
-
-
-// ************************
-// *** Helper functions ***
-// ************************
-int constrainESC(float input)
-{
-    if (input < 1100.0)
-        return 1100;
-    else if (input > 2000.0)
-        return 2000;
-    else
-        return (int) input;
-}

--- a/main.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,40 +0,0 @@
-/* File:    main.cpp
- * Author:  Trung Tin Ian HUA
- * Date:    May 2014
- * Purpose: Main code to initialis all devices and then start threads.
- */
-#include "mbed.h"
-#include "rtos.h"
-#include "setup.h"
-#include "tasks.h"
-
-int main(void)
-{
-    bool error = false;
-
-    error = setupALLdevices();
-    if (!error) {
-        PC.printf("\n\nQuadcopter READY! Mode: ");
-        BT.printf("\n\nQuadcopter READY! Mode: ");
-        switch (mode) {
-            case RATE:
-                PC.printf("RATE\n");
-                BT.printf("RATE\n");
-                break;
-            case ATTITUDE:
-                PC.printf("ATTITUDE\n");
-                BT.printf("ATTITUDE\n");
-                break;
-            default:
-               // imu.debugSerial.printf("UNKNOWN\n");
-                BT.printf("UNKNOWN\n");
-                break;
-        }
-        createThreads();
-    } else {
-        PC.printf("\n\nSetup failed!\n");
-        BT.printf("\n\n!!!Setup FAILED!!!\n");
-        return -1;
-    }
-    return 0;
-}