File content as of revision 4:4324f20e4597:

#include "mbed.h"
#include "Barometer.h"
#include "LocalFileSystem.h"
#include "math.h"
#include "Servo.h"
#define dt 0.1

Serial pc(USBTX, USBRX);
Ticker blue_trig;
Timer end;

////////////////////////////////////////
//   Bluetooth    3.3V p13 p14(TX,RX) //
////////////////////////////////////////
Serial Blue(p13, p14);
int send_ok=0;
char Blue_msg[150];
int k=0, Blue_ok=0, Blue_flag=0;
volatile unsigned char Blue_buffer[2];
float input_roll, input_pitch, input_yaw, input_thr;

void Blue_isr(){                      //inturupt 
    while(Blue.readable()){
        Blue_buffer[1] = Blue_buffer[0];
        Blue_buffer[0] = Blue.getc();
        if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
        if (Blue_buffer[0] == '*'){Blue_flag=1;}
        if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
    }
} 

void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
{
    if (Blue_ok == 1){
        Blue_ok = 0;
        sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
    }
} 

void blue_trig_isr(){
    send_ok=1;   
}

void trans_blue_data(float in_data, int integer_point, int under_point){  // number of intefer and under_point
        unsigned int conv_trans_data;
        conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
        if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
        else {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '+';}
        for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
            if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.';   }        
            while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48);       //convert to ASCII    
        }
}

//Bluetooth code is placed under the Log_data

////////////////////////////////////////////
//    Barometer     3.3V p9(SDA) p10(SCL) //  
////////////////////////////////////////////
Barometer barometer(p9, p10);
float alt=0.0;
float alt_sum=0.0f, alt_zero=0.0f;
int count = 0, baro_ok = 0;          // for zero-calibration
float alt_buffer[2], w_alt=0;          // weight for LPF

void get_Baro(float*alt)
{   
    if (baro_ok==1){
        barometer.update();
        *alt = barometer.get_altitude_m();
        alt_buffer[1] = alt_buffer[0]; 
        alt_buffer[0] = *alt;
        if(abs(alt_buffer[0]- alt_buffer[1])>20){
            *alt = alt_buffer[1];
            baro_ok = 0;
        }
        else{
            baro_ok = 0;
        }
    }
} 

void calb_alt(){
    if (alt==0){count=0;}
    else {
        if (count==1){count++;}
        else{
            if (count<=99){alt_sum = alt_sum + alt; count++;}
            else {alt_zero = alt_sum/(float)(count-1); count++;}
        }
    }
}

///////////////////////////////////////
//         AHRS          5V p27(RX)  //   20Hz
///////////////////////////////////////
Serial AHRS(p28, p27);
float roll,pitch,yaw,velx,vely,velz,velxyz;
char AHRS_msg[150];
int m=0, ahrs_ok=0, AHRS_flag=0;
volatile unsigned char AHRS_buffer[2];

void AHRS_isr(){                      //inturupt 
    while(AHRS.readable()){
        AHRS_buffer[1] = AHRS_buffer[0];
        AHRS_buffer[0] = AHRS.getc();
        if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
        if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
        if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
    }
} 

void get_AHRS(float*roll, float*pitch, float*yaw, float*velx, float*vely, float*velz, float*velxyz)
{
    if (ahrs_ok == 1){
        ahrs_ok = 0;
        sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
        *velxyz = (float)sqrt(pow(*velx,2)+pow(*vely,2)+pow(*velz,2));
        baro_ok = 1;
    }
} 

///////////////////////////////////
//      Servo         5V  PWM    // needed to check pin#
///////////////////////////////////
Servo Throttle(p26);
Servo CS1(p25); //below
Servo CS2(p23); //upper
Servo CS3(p21); //below
Servo CS4(p22); //upper

float thr_value = 0.0, ctrl1_value = 0.5, ctrl2_value = 0.5, ctrl3_value = 0.5, ctrl4_value = 0.5;
float err_roll = 0.0, err_pitch = 0.0, err_yaw = 0.0, err_alt = 0.0, preerror_roll = 0.0, preerror_pitch = 0.0, preerror_yaw = 0.0;
float ctrl_roll = 0.0, ctrl_pitch = 0.0, ctrl_yaw = 0.0;
float kp1=1.0, kd1=0.0, kp2=1.0, kd2=0.0, kp3=1.0, kd3=0.0;   // PD Controller

void ctrl_attitude(){
    err_roll = input_roll - roll;
    err_pitch = input_pitch - pitch;
    err_yaw = input_yaw - yaw;
    ctrl_roll = (kp1*err_roll)+(kd1*(err_roll-preerror_roll)/dt);
    ctrl_pitch = (kp2*err_pitch)+(kd2*(err_pitch-preerror_pitch)/dt);
    ctrl_yaw = (kp3*err_yaw)+(kd3*(err_yaw-preerror_yaw)/dt);
    ctrl_roll = (ctrl_roll/180.0); // Range adjustment
    ctrl_pitch = (ctrl_pitch/180.0);
    ctrl_yaw = (ctrl_yaw/180.0);
    preerror_roll = err_roll;
    preerror_pitch = err_pitch;
    preerror_yaw = err_yaw;
    // Roll = ctrl2 - ctrl4
    // Pitch = ctrl1 - ctrl3
    // Yaw = ctrl1 + ctrl2 + ctrl3 + ctrl4
    ctrl1_value = ((ctrl_pitch/2.0) + (ctrl_yaw/4.0)) + 0.5;
    ctrl2_value = ((ctrl_roll/2.0) + (ctrl_yaw/4.0)) + 0.5;
    ctrl3_value = (-(ctrl_pitch/2.0) + (ctrl_yaw/4.0)) + 0.5;
    ctrl4_value = (-(ctrl_roll/2.0) + (ctrl_yaw/4.0)) + 0.5;
    if (ctrl1_value<=0.0){ctrl1_value=0.0;}
    else if (ctrl1_value>=1.0) {ctrl1_value=1.0;}
    if (ctrl2_value<=0.0){ctrl2_value=0.0;}
    else if (ctrl2_value>=1.0) {ctrl2_value=1.0;}
    if (ctrl3_value<=0.0){ctrl3_value=0.0;}
    else if (ctrl3_value>=1.0) {ctrl3_value=1.0;}
    if (ctrl4_value<=0.0){ctrl4_value=0.0;}
    else if (ctrl4_value>=1.0) {ctrl4_value=1.0;}
    CS1 = ctrl1_value;
    CS2 = ctrl2_value;
    CS3 = ctrl3_value;
    CS4 = ctrl4_value;
}

 
void neutral(){
    ctrl1_value = 0.5;
    ctrl2_value = 0.5;
    ctrl3_value = 0.5;
    ctrl4_value = 0.5;
    CS1 = ctrl1_value;
    CS2 = ctrl2_value;
    CS3 = ctrl3_value;
    CS4 = ctrl4_value;
}

///////////////////////////////
//   Datalogger       Mbed   //
///////////////////////////////
LocalFileSystem local("local");
int file_no=1;
char filename[256];
FILE *fp;

void Log_file(){
    sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
    fp = fopen(filename, "r"); // if file can be loaded, return is 1
    while(fp){
        fclose(fp);
        file_no ++;
        sprintf(filename, "/local/Data%d.txt", file_no);    // Open "tem%d.txt" on the local file system for writing
        fp = fopen(filename, "r");
    }
    fp = fopen(filename, "w");    
}

void Log_data(){
    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",roll,pitch,yaw,alt,input_roll,input_pitch,input_yaw,input_thr,velx,vely,velz);
}

void send_Blue(){
    if (send_ok == 1){
        send_ok = 0;
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
        trans_blue_data(roll,3,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
        trans_blue_data(pitch,3,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
        trans_blue_data(yaw,3,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
        trans_blue_data(alt,2,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
        trans_blue_data(velxyz,2,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
        trans_blue_data(velz,2,2);
        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
    }
}


/////////////////////////////////
//         Main loop           // 
/////////////////////////////////

int main(void)
{   
    AHRS.baud(9600);
    Blue.baud(9600);
    Blue.attach(&Blue_isr);
    AHRS.attach(&AHRS_isr);
    blue_trig.attach(&blue_trig_isr, 0.1);
    neutral();
    Log_file();
        while(1) {
                get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
                get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
                get_Baro(&alt);
                if (input_roll==0){input_roll = roll;}
                if (input_pitch==0){input_pitch = pitch;}
                if (input_yaw==0){input_yaw = yaw;}
                ctrl_attitude();
                Log_data();
                send_Blue();
                if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){fclose(fp); break;}
    }
}