File content as of revision 0:6ac6b2d2bf1a:

#include "mbed.h"
#include "Barometer.h"
#include "Ultrasonic.h"
#include "math.h"
Serial pc(USBTX, USBRX);
Barometer barometer(p9, p10);
Ultrasonic sonar(p24, p12); 
Serial AHRS(p13, p14);
Serial GPS(p28, p27);
PwmOut Linear(p21);
PwmOut Micro_gf(p22);
PwmOut Micro_d(p23);

float roll,pitch,yaw,accx,accy,accz;
float p = 0.0f, t = 0.0f, alt = 0.0f;
int i = 0, j=0, gps_ok=0, flag, lock
int stat, chksum=0;
volatile unsigned char GPS_buffer[2];
char ns, ew;
char msg[150];

void GPS_isr(){
    i++;
    GPS_buffer[1] = GPS_buffer[0];
    GPS_buffer[0] = 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;
}

int main(void)
{   AHRS.baud(9600);
    GPS.baud(9600);
    GPS.attach(&GPS_isr, Serial::RxIrq);
    float longitude,latitude, time;
    Linear.period(0.01); // set PWM period to 1ms
    Micro_gf.period(0.01);
    Micro_d.period(0.01);
    //Linear=0.1;wait(1);
    Linear=0.19;
    Micro_d=0.2;
    Micro_gf=0.2;
    wait(1);
    while(1) {
        //stat should be added
        barometer.update();
        p = barometer.get_pressure();
        t = barometer.get_temperature();
        alt = barometer.get_altitude_m();
        while(AHRS.getc() != '\n');
        AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", &roll, &pitch, &yaw, &accx,&accy,&accz);
        if (gps_ok == 1)
        {gps_ok = 0;
         sscanf(msg, "GA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &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;
            time = time + 90000;
        }
        unsigned int distance = sonar.distance();
        if (distance<10){Linear =0.1;}
        chksum=(int)stat+t+alt+roll+pitch+yaw+accz+time+latitude+longitude+distance;
     pc.printf("\r\n Temperature(c):%f Altitude(m):%f Roll:%f Pitch:%f Yaw:%f Accz:%f Time:%f Latitude:%f Longitude:%f Sonar(cm)%u",stat,t,alt,roll,pitch,yaw,accz,time,latitude,longitude,distance,chksum);
    }   
}