File content as of revision 1:cd11c1c592c7:

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

Serial pc(USBTX, USBRX);
int data[14];
int stat=1;

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

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 (flag == 1){flag = 0;gps_ok = 1;j=0;}
    }
    if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
    if (flag==1){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(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;
    }
}

///////////////////////////////////////
//         AHRS          5V p14(RX)  //
///////////////////////////////////////
Serial AHRS(p13, p14);
float roll,pitch,yaw,accx,accy,accz;
float t, alt;

void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
{
    while(AHRS.getc() != '\n');
    AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", roll, pitch, yaw, accx, accy, accz);
}       
   
////////////////////////////////////////////
//    Barometer     3.3V p9(SDA) p10(SCL) //
////////////////////////////////////////////
Barometer barometer(p9, p10);
float alt_sum=0.0f, alt_zero=0.0f, alt_avg=0.0f;  // for zero-calibration
float alt_buffer[5];
int count = 0;

void get_Baro(float*t, float*alt)
{
    barometer.update();
    *t = barometer.get_temperature();
    *alt = barometer.get_altitude_m();
} 

void Baro_zero(){
    if (count<=4){
        alt_sum = alt_sum + alt;
        count++;
    }
    else {
        pc.printf("calibration\r\n");
        alt_zero = alt_sum / (float)count ;
    }
}

/*
void Baro_check(){
    alt_buffer[0] = alt;
    for(i=0;i<=3;i++) {alt_buffer[i+1] = alt_buffer[i]; } //Save in 5 buffers
    del_alt = alt_min - alt_max;
}
*/

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

///////////////////////////////////
//           Checksum            //
///////////////////////////////////
long chksum;
unsigned int checksum (int data, short message_length)
{
   unsigned int  sum = 10;
   /*for (i=0;i<=message_length; i++){
      sum += data[i];
   }
    sum =  (sum & 0xff);*/
    return (unsigned int)(~sum);
}

void packet_data(){
    data[0] = stat;
    data[1] = Kor_time;
    data[2] = (int)(alt*100);
    data[3] = (int)(alt_GPS*100);
    data[4] = (int)(roll*100);
    data[5] = (int)(pitch*100);
    data[6] = (int)(yaw*100);
    data[7] = (int)(accz*100);
    data[8] = (int)(latitude*100);
    data[9] = (int)(longitude*100);
    data[10] = Micro_unf*100;
    data[11] = Micro_gf*100;
    data[12] = Linear*100;
    data[13] = chksum;
}

///////////////////////////////
//   Datalogger       Mbed   //
///////////////////////////////
LocalFileSystem local("local");
int num=0;
FILE *fp;

void Log_data(){
    FILE *fp = fopen("/local/data.txt", "w"); 
    for (num=0;num<=13;num++) {fprintf(fp, "%i ",data[num]);}
    num=0;
    fprintf(fp, "\r\n");
}

int main(void)
{   
    wait(10);
    AHRS.baud(9600);
    Blue_GPS.baud(9600);
    Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
    Linear.period(0.01); Micro_gf.period(0.01); Micro_unf.period(0.01);
    Linear=0.19; Micro_unf=0.2; Micro_gf=0.2;//Initial setting of servo
    //Linear=0.1;wait(1);Take back
    pc.printf("Start\r\n");
        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(&t, &alt);
                Baro_zero();
                packet_data();
                //Log_data();
                pc.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero);
                //if (Kor_time!=0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
                if (5<=count){Baro_zero(); pc.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero); stat=2;}
                break;
            case 2 :   //Waiting launch
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&t, &alt);
                alt = alt - alt_zero;
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                packet_data();
                pc.printf("%i %i %i %i %i %i %i %i %i %i %i %i %i %i %i %i\r\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11],data[12],data[13]);
                if (100.0<alt_GPS && accz<0){stat=3;}
                break;
            case 3 :   //Grid fin and landing leg spreading and control
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&t, &alt);
                alt = alt - alt_zero;
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                packet_data();
                if (alt<3){stat=4;}
                break;
            case 4 :   //Reverse thrust for soft landing
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&t, &alt);
                alt = alt - alt_zero;
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                packet_data();
                if (accx<0.1 && accy<0.1 && accz<0.1) {stat=5;}
                break;
            case 5 :   //Landing
                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                get_Baro(&t, &alt);
                alt = alt - alt_zero;
                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                packet_data();
                break;
        }
    }
}