File content as of revision 0:7f6319a73219:

// Calibrate and arm ESC and then sweep through motor speed range
// To run: Hold down reset on mbed and power up ESC motor supply
// Wait for three power up beeps from ESC, then release reset (or apply power) on mbed
// See https://github.com/bitdump/BLHeli/blob/master/BLHeli_S%20SiLabs/BLHeli_S%20manual%20SiLabs%20Rev16.x.pdf
// for info on beep codes and calibration
#include "mbed.h"
#include "Servo.h"
#include "MPU6050.h"
#include "ledControl.h"
#include "PID.h"

PwmOut ledf(LED1); //throttle up test led with PWM dimming
PwmOut ledr(LED2); //throttle down test led with PWM dimming

Servo ESC0(p21); //Back  Left
Servo ESC1(p22); //Front Left
Servo ESC2(p24); //Front Right
Servo ESC3(p23); //Back  Right

Serial pc(USBTX, USBRX); //Initalise PC serial comms
Serial Receiver(p13, p14); //Initialize Receiver Serial comms
MPU6050 mpu6050;           // class: MPU6050, object: mpu6050
Ticker toggler1;
Ticker filter;

//using namespace std::chrono;

void toggle_led1();
void toggle_led2();
void compFilter();

float pitchAngle = 0;
float rollAngle = 0;
float yawAngle = 0;

float pitchTarget = 0;
float rollTarget = 0;
float yawTarget = 0;

PID PitchPID(0.5,1,0.01);
PID RollPID(0.5,1,0.01);
PID YawPID(0.5,1,0.01);

float ThrustValue = 0;

// Receiver variables
uint16_t channel[14];

Timer t;

int main()
{
    ESC0 = ESC1 = ESC2 = ESC3 = 0;
    ledf = ledr = 0;
    ESC0 = ESC1 = ESC2 = ESC3 = 0;
    wait(0.5);
    ESC0 = ESC1 = ESC2 = ESC3 = 0.2;
    wait(0.5);
    ESC0 = ESC1 = ESC2 = ESC3 = 0;
    wait(0.5);

    uint8_t byte1, byte2;
    //bool ready_to_arm = false;
    pc.baud (115200);
    Receiver.baud(115200);

    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading
    wait(1);
    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
    pc.printf("Calibration is completed. \r\n");
    wait(0.5);
    mpu6050.init();                             // Initialize the sensor
    wait(1);
    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
    wait_ms(500);
    filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
    rollTarget = rollAngle;
    pitchTarget = pitchAngle;
    yawTarget = yawAnglw;

    PitchPID.setLimit(-0.25,0.25);
    RollPID.setLimit(-0.25,0.25);
    YawPID.setLimit(-0.25,0.25);

    pc.printf("Ready to go...\n\r");

    float rollAdjustment = 0;
    float pitchAdjustment = 0;
    float yawAdjustment = 0;

    while (1) {
        t.start();

        //PitchPID.setProcessValue(pitchAngle);
        //pc.printf("| Pitch: %.5f | Output:%.5f |\r\n",pitchAngle,PitchPID.compute());

        //PitchPID.setProcessValue(rollAngle);
        //pc.printf("| Roll: %.5f | Output:%.5f |\r\n\n",rollAngle,PitchPID.compute());

        //float imuRoll = 0;
        //float imuPitch = 0;

        if (Receiver.readable()) {
            if(Receiver.getc() == 0x20) {
                if(Receiver.getc() == 0x40) {
                    //printf("Message Recieved! \n");
                    for (int i = 0; i<3; i++) {
                        byte1 = Receiver.getc();
                        //byte2 = Receiver.getc();
                        //if (i == 2)
                        //    printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
                        channel[i] = (Receiver.getc() << 8);
                        channel[i] += (byte1&0x000000FF);
                    }

                    //channel[2] -= 1000; //Motor
                    ThrustValue = (channel[2]-1000)*0.001;

                    rollAdjustment = RollPID(rollTarget,rollAngle);
                    // Limit roll adjustment if needed

                    pitchAdjustment = PitchPID(pitchTarget,pitchAngle);
                    // Limit pitch adjustment if needed

                    yawAdjustment = YawPID(yawTarget,yawAngle);
                    // Limit pitch adjustment if needed
                }
            }
            t.stop();
            pc.printf("The time taken was %.5f seconds\n\r", t.read());
            UpdateMotors(ThrustValue, yawAngle, pitchAngle, rollAngle);
        }
    }
}

void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
{
    //Back Left
    ESC0 = Thrust + Yaw - Pitch - Roll;

    //Front Left
    ESC1 = Thrust - Yaw + Pitch - Roll;

    //Front Right
    ESC2 = Thrust + Yaw + Pitch + Roll;

    //Back Right
    ESC3 = Thrust + Yaw - Pitch + Roll;
}

//float MotorCheck(float Motor){

//}

void toggle_led1()
{
    ledToggle(1);
}
void toggle_led2()
{
    ledToggle(2);
}

/* This function is created to avoid address error that caused from Ticker.attach func */
void compFilter()
{
    mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);

}