File content as of revision 2:91f2479501a0:

// 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 "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

uint16_t motor_min = 1100;

DigitalOut led1(LED1);
void Rx_interrupt();

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

//using namespace std::chrono;

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

int arm = 0;

uint16_t pitchTarget = 0;
uint16_t rollTarget = 0;
//float yawTarget = 0;

float pitchOffset = 0;
float rollOffset = 0;

PID PitchPID(0.5,0,0.05);
PID RollPID(0.5,0,0.05);
PID YawPID(2,0.01,0);

float Throttle = 0;

// Receiver variables
int16_t channel[14];

float pid_setpoint_roll = 0;
float pid_setpoint_pitch = 0;
float pid_setpoint_yaw = 0;
float pid_output_roll = 0;
float pid_output_pitch = 0;
float pid_output_yaw = 0;

int16_t gr[3];
int16_t acc[3];
int16_t grC[3];
int16_t accC[3];



Timer t;
Timer loop_time;
Timer re;

void Calculate_set_points(){
	
	// Throttle- CHannel[2]
	if (channel[2] > 1800){Throttle = 1800;}
	else{Throttle = channel[2];}
	// Roll - Y - Channel[0]
	if (channel[0] != 0){
		if (channel[0] > 1505){pid_setpoint_roll =  channel[0] - 1505;} 	// max 495
		if (channel[0] < 1495){pid_setpoint_roll =  channel[0] - 1495;} 	// max -495
		pid_setpoint_roll /= 1;	// Max angular rate of 247 deg/s
	}
	else {
		pid_setpoint_roll = 0;
	}
	
	// Pitch - X - Channel[1]
	if (channel[1] != 0){
		if (channel[1] > 1505){pid_setpoint_pitch = 1505 - channel[1];} 	// max 495
		if (channel[1] < 1495){pid_setpoint_pitch =  1495 - channel[1];} 	// max -495
		pid_setpoint_pitch /= 1;	// Max angular rate of 247 deg/s
	}
	else {
		pid_setpoint_roll = 0;
	}
	
	//Yaw - Z - Channel[3]
	if (channel[3] != 0){
		if (channel[3] > 1505){pid_setpoint_yaw = 1505 - channel[3];} // max 495
		if (channel[3] < 1495){pid_setpoint_yaw = 1495 - channel[3];} // max -495
		pid_setpoint_roll /= 1;		// Max angular rate of 247 deg/s
	}
	else{
		pid_setpoint_yaw = 0;
		}
		
	//pc.printf("Pset: %.1f, Rset %.1f, Yset %.1f ",pid_setpoint_roll, pid_setpoint_roll, pid_setpoint_roll); 
}

// Interupt Routine to read in data from serial port
void Rx_interrupt(){
	uint8_t byte1;
	uint8_t high_byte;
	uint8_t chksum_low_byte;
	uint8_t chksum_high_byte;
	if (Receiver.readable()) {
            if(Receiver.getc() == 0x20) {
                    if(Receiver.getc() == 0x40){
                    for (int i = 0; i<7; 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);
                	}
                	chksum_low_byte = Receiver.getc();
                	chksum_high_byte = Receiver.getc();
                	//pc.printf("CLB %u,CHB %u \n\r", chksum_low_byte, chksum_high_byte);
                	//pc.printf("CN1: %u, CN2: %u, CN3: %u \n\r", channel[1], channel[2], channel[3]);
                	}
            }
        }
}

void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
{
    // ESC0 |  Back Left   | CCW
    // ESC1 |  Front Left  | CW
    // ESC2 |  Front Right | CCW
    // ESC3 |  Back Right  | CW


    //Back Left
    float SC0 = 0;
    SC0 = Thrust - Yaw - Pitch + Roll;
    if(SC0 < motor_min) {
        SC0 =  motor_min;
    }
    if(SC0 > 2000) {
        SC0 = 2000;
    }
    float NSC0 = (SC0-1003)/1003;
    ESC0 = NSC0;

    //Front Left
    float SC1 = Thrust + Yaw + Pitch + Roll;
    if(SC1 < motor_min) {
        SC1 =  motor_min;
    }
    if(SC1 > 2000) {
        SC1 = 2000;
    }
    float NSC1 = (SC1-1003)/1003;
    ESC1 = NSC1;

    //Front Right
    float SC2 = Thrust - Yaw + Pitch - Roll;
    if(SC2 < motor_min) {
        SC2 =  motor_min;
    }
    if(SC2 > 2000) {
        SC2 = 2000;
    }
    float NSC2 = (SC2-1003)/1003;
    ESC2 = NSC2;

    //Back Right
    float SC3 = Thrust + Yaw - Pitch - Roll;
    if(SC3 < motor_min) {
        SC3 =  motor_min;
    }
    if(SC3 > 2000) {
        SC3 = 2000;
    }
    float NSC3 = (SC3-1003)/1003;
    ESC3 = NSC3;


    //pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f ", SC0, SC1, SC2, SC3);
    //pc.printf("NBL:%.3f, NFL:%.3f, NFR:%.3f, NBR:%.3f ", NSC0, NSC1, NSC2, NSC3);
}

int main()
{
    // Setup a serial interrupt function to receive data
    led1 = 1;
    Receiver.baud(115200);
    pc.baud (115200);
    Receiver.attach(&Rx_interrupt);

    for (int c = 0; c<14; c++) {
        channel[c] = 0;
    }
    ESC0 = ESC1 = ESC2 = ESC3 = 0;
    wait(0.5);
    ESC0 = ESC1 = ESC2 = ESC3 = 1100;
    wait(0.5);
    ESC0 = ESC1 = ESC2 = ESC3 = 0;
    wait(0.5);
    

   //
    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading
    wait(1);
    mpu6050.init();    
    wait(0.5);                         // Initialize the sensor
    mpu6050.calibrate(accelBias,gyroBias);    // Calibrate MPU6050 and load biases into bias registers
    pc.printf("Calibration is completed. \r\n");
    wait(1);
    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
    wait_ms(500);
    
    


    //pc.printf("%f ,%f",Roll Angle, pitchAngle)

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

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



    while (1) {
        loop_time.start();
        t.start();
        
        // TO ADD
        // Timer timeout for reciving new commands, if after 10 s no new commands cut motor output
// AUTO LEVEL 
		// Read MPU Values with complimantary filter allready applied
        //mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
        //pc.printf("Pitch: %.3f, Roll: %.3f, Yaw: %.3f \n\r", pitchAngle, rollAngle, yawAngle);
        
// RATE MODE
		mpu6050.readAccelData(acc);
		mpu6050.readGyroData(gr);
		accC[0] = ((acc[0]-accelBias[0])/16384);
		grC[0] = ((gr[0]-gyroBias[0])/65.536);
		
		accC[1] = ((acc[1]-accelBias[1])/16384);
		grC[1] = ((gr[1]-gyroBias[1])/65.536);
		
		accC[2] = ((acc[2]-accelBias[2])/16384);
		grC[2] = ((gr[2]-gyroBias[2])/65.536)*-1;
		
		pitchAngle =  (pitchAngle*0.7) + (grC[0]*0.3);
		rollAngle =  (rollAngle*0.7) + (grC[1]*0.3);
		yawAngle =  (yawAngle*0.7) + (grC[2]*0.3);

		//pc.printf("ac_x: %d ac_y %d ac_z %d gy_x %d gy_y %d gy_z %d  \r\n", accC[0], accC[1], accC[2], grC[0], grC[1], grC[2]);
        
        Calculate_set_points();
        
        // Set Reciver channels 
        

        //pc.printf("T:%.1f R:%.1f P:%.1f ARM: %u \n\r",Throttle, pid_setpoint_roll, pid_setpoint_pitch, channel[6]);
        
        // Set PIDS
        pid_output_roll = RollPID.computeOutput(pid_setpoint_roll,rollAngle);
        pid_output_pitch = PitchPID.computeOutput(pid_setpoint_pitch,pitchAngle);
        pid_output_yaw = YawPID.computeOutput(pid_setpoint_yaw,yawAngle);
        //pc.printf("Radj: %.1f, Padj %.1f ",pid_output_roll, pid_output_pitch); 
        // Yaw angle is currently very broken and constantly rises (Check MPU6050 complimentry filter function as it looks
        // like it doesnt apply the z axis config offset values)
        //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
        
        
        // Update Motor PWM if the arming switch is triggered
        if (channel[6] > 1600){UpdateMotors(Throttle,pid_output_yaw, pid_output_pitch, pid_output_roll);}
        else{
        	ESC0 = 0;
        	ESC1 = 0; 
        	ESC2 = 0;
        	ESC3 = 0;
        }
        

		
        



        
        



        //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
        //yawAdjustment*=Ykc;
        //printf("Adj: %f, Targ: %f, Ang: %f\n\r",yawAdjustment,yawTarget,yawAngle);
        //printf("%f\n\r",yawAdjustment);
        // Limit pitch adjustment if needed

        //if (arm) {}

        t.stop();
        //pc.printf("Loop time %f \n\r", t.read());
        if(t.read()*1000 > 3) {
            pc.printf("LOOP TIME TOO LONG!");
            led1 = 1;
        } else {
            wait_ms(3.04-t.read()*1000);
        }
        t.reset();
        loop_time.stop();
        //pc.printf("Loop time %f \n\r", loop_time.read());
        loop_time.reset();
    }
}

//float MotorCheck(float Motor){

//}