File content as of revision 3:e3e965924dde:

#include "mbed.h"
#include "Barometer.h"
#include "LocalFileSystem.h"
#include "math.h"
#include "Servo.h"

Ticker blue_trig;
Timer end;
int stat=1;

////////////////////////////////////
//         GPS         5V p27(RX) //  
//      Bluetooth    3.3V p28(TX) //
////////////////////////////////////
Serial Blue_GPS(p28, p27);
char GPS_msg[150], ns, ew;
int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
volatile unsigned char GPS_buffer[2];
float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
int blue_ok=0;

void GPS_isr(){
    i++;
    GPS_buffer[1] = GPS_buffer[0];
    GPS_buffer[0] = Blue_GPS.getc();
    if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){   
        i=0;
        if (GPS_flag == 1){GPS_flag = 0;gps_ok = 1;j=0;}
    }
    if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
    if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
}

float trunc(float v) {
    if(v < 0.0) {
        v*= -1.0;
        v = floor(v);
        v*=-1.0;
    }
    else {v = floor(v);}
    return v;
}

void get_GPS(float *Kor_time, float *latitude, char *ns, float *longitude, char *ew, float *fix, float *sat, float *x, float *alt_GPS, float *lock)
{
    if (gps_ok == 1){
        gps_ok = 0;
        sscanf(GPS_msg, "GA,%f,%f,%c,%f,%c,%f,%f,%f,%f,%f", Kor_time,latitude,ns,longitude,ew,fix,sat,x,alt_GPS,lock);
        if(*ns == 'S') {*latitude  *= -1.0; }
        if(*ew == 'W') {*longitude *= -1.0; }
        float degrees = trunc(*latitude / 100.0f);
        float minutes = (*latitude - (degrees * 100.0f));
        *latitude = degrees + minutes / 60.0f;    
        degrees = trunc(*longitude / 100.0f);
        minutes = *longitude - (degrees * 100.0f);
        *longitude = degrees + minutes / 60.0f;
        *Kor_time = *Kor_time + 90000;
    }
}

void blue_trig_isr(){
    blue_ok=1;   // AHRS 20HZ
}

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_UART2->LSR&0x20)==0); LPC_UART2->THR = '-';}
        else {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '+';}
        for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
            if(cnt_num == under_point) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '.';   }        
            while((LPC_UART2->LSR&0x20)==0); LPC_UART2->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 t, alt, del_alt;
float alt_sum=0.0f, alt_zero=0.0f;
int count = 0, baro_ok = 0;          // for zero-calibration
float alt_buffer[4], w_alt=0.6;          // weight for LPF

void get_Baro(float*alt, float*del_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])>100){
            alt_buffer[0] = alt_buffer[1];
            *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
            *del_alt = 0;      // for calculation of drop speed
            baro_ok = 0;
        }
        else{
            *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
            alt_buffer[3] = alt_buffer[2];
            alt_buffer[2] = *alt;
            *del_alt = alt_buffer[3]-alt_buffer[2];      // for calculation of drop speed
            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-2);
                count++;
            }
        }
    }
}

///////////////////////////////////////
//         AHRS          5V p14(RX)  //   20Hz
///////////////////////////////////////
Serial AHRS(p13, p14);
float roll,pitch,yaw,accx,accy,accz;
char AHRS_msg[150];
int m=0, ahrs_ok=0, AHRS_flag=0;
volatile unsigned char AHRS_buffer[2];
float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.7;

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*accx, float*accy, float*accz)
{
    if (ahrs_ok == 1){
        ahrs_ok = 0;
        sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
        roll_buffer[1] = roll_buffer[0];
        roll_buffer[0] = *roll;
        *roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0];  // Low Pass Filter 
        pitch_buffer[1] = pitch_buffer[0];
        pitch_buffer[0] = *pitch;
        *pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0];  // Low Pass Filter 
        yaw_buffer[1] = yaw_buffer[0];
        yaw_buffer[0] = *yaw;
        *yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0];  // Low Pass Filter 
        baro_ok = 1;
    }
} 

///////////////////////////////////
//      Servo         5V  PWM    //
///////////////////////////////////
Servo Micro_unf(p21);
Servo Micro_gf(p22);
Servo Linear(p23);

float unf_value=1.0, gf_value = 0.35, linear_value = 0.25;
float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;

void ctl_gf(){
    err_yaw = yaw - tg_yaw;
    gf_value = 0.35*exp(0.0039*p*err_yaw);
    if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
    if (gf_value<=0.0){gf_value=0.0;}
    else if (gf_value>=0.7) {gf_value=0.7;}
    Micro_gf = gf_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, "%i,%.0f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
}

void send_Blue(){
    if (blue_ok == 1){
        blue_ok = 0;
        trans_blue_data((float)stat,1,0);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; // basic formation of way to send data
        trans_blue_data(Kor_time,6,0);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(latitude,2,6);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 
        trans_blue_data(longitude,3,6);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(alt,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(del_alt,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(alt_GPS,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(roll,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(pitch,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(yaw,3,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(accz,2,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(unf_value,1,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(gf_value,1,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
        trans_blue_data(linear_value,1,2);
        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '\n';
    }
}


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

int main(void)
{   
    AHRS.baud(9600);
    Blue_GPS.baud(9600);
    Blue_GPS.attach(&GPS_isr);
    AHRS.attach(&AHRS_isr);
    blue_trig.attach(&blue_trig_isr, 0.125); 
    Micro_gf = gf_value; //Initial setting of servo
        while(1) {
        switch(stat){
            case 1 :   //Bluetooth connection and calibration
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&alt, &del_alt);
                calb_alt();
                send_Blue();
                if (100<count) {stat=2;}
                break;
            case 2 :   //Wait for launch and keep the order get_AHRS, it sets trigger of get_Baro and send_Blue and Log-data save raw_alt
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&alt, &del_alt);
                alt = alt - alt_zero;
                send_Blue();
                if (100.0<alt_GPS && del_alt>1){
                    stat=3;
                    Log_file();
                    Micro_unf = unf_value;
                    unf_value = 0.0;   //checking
                    wait(0.5);
                    Micro_gf = gf_value;
                }
                break;
            case 3 :   //Grid fin and landing leg spreading and control
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&alt, &del_alt);
                Log_data();
                alt = alt - alt_zero;
                send_Blue();
                if (alt<10){ // need to modify trigger 
                    fclose(fp);
                    Log_file();
                    stat=4;
                    Linear=linear_value;
                    linear_value=0.0;
                } 
                break;
            case 4 :   //Reverse thrust for soft landing
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&alt, &del_alt);
                Log_data();
                alt = alt - alt_zero;
                send_Blue();
                if (accx<0.1 && accy<0.1 && alt<3){
                    fclose(fp);
                    Log_file();
                    end.start();
                    stat=5;
                }
                break;
            case 5 :   //Landing
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&alt, &del_alt);
                Log_data();
                alt = alt - alt_zero;
                send_Blue();
                end.read();
                if (end.read()>=10) {
                    fclose(fp);
                    stat=6;
                }
                break;
            case 6 :   //Shut down
                break;
        }
    }
}