File content as of revision 5:ce6688d37d12:

#include "mbed.h"
#include "MMA8451Q.h"
#include "TinyGPS.h"
#include "HMC5883L.h"
#include "PwmIn.h"
 
#define MMA8451_I2C_ADDRESS (0x1d<<1)

I2C i2c_bus(PTE0, PTE1);
MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
HMC5883L compass(i2c_bus);
Serial pc(USBTX, USBRX);
Serial radio(PTC4, PTC3);
Timer t;
TinyGPS gps;
Serial gpsSerial(PTD3,PTD2);
PwmIn rudderIn(PTD0);
PwmIn thrusterIn(PTD5);
PwmOut rudderOut(PTA4);
PwmOut thrusterOut(PTA5);

void gpsSerialRecvInterrupt (void);
void radioSerialRecvInterrupt (void);
void radioFlush(void);

bool manual = true;
int commandLength = 2;
//radio commands
enum{ thruster='a',
    rudder,
    combo,
    control};
    
float rudderC = 0.0015, thrusterC = 0.0015;

int main(void) {

    
    pc.baud(115200);
    radio.baud(57600);
    
    pc.printf("pre - inited\r\n");
    radio.printf("pre - inited\r\n");
    int16_t dCompass[3];
    float dAcc[3];
    compass.init();
    pc.printf("inited\r\n");
    gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq);    // Recv interrupt handler
    radio.attach (&radioSerialRecvInterrupt, radio.RxIrq);    // Recv interrupt handler
    gps.reset_ready();
    rudderOut.period(0.02);
    thrusterOut.period(0.02);
    rudderOut.pulsewidth(0.0015);
    thrusterOut.pulsewidth(0.0015);


 
    while (true) {
        
        if(gps.gga_ready()){
           
            pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
            radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
        }

        acc.getAccAllAxis(dAcc);
        pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
        radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
        compass.getXYZ(dCompass);
        pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
        radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
        if(manual){
            wait(0.2);
            rudderOut.pulsewidth(rudderIn.pulsewidth());
            thrusterOut.pulsewidth(thrusterIn.pulsewidth());
            }
        else{
            wait(0.2);
            //rudderC should be an angle -45 to 45
            //map this to 0.0013 to 0.0017
            if(rudderC > -45 && rudderC < 45){
                rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90));
            }
            //thrusterC should be an percentage from 0 to 100
            //map this to 0.0013 to 0.0017
            thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100));
        }
    }
}

//*****************************************************************************
//  serial receive interrupt handler
//***************************************************************************** 

void gpsSerialRecvInterrupt (void)
{
    

   // while(gpsSerial.readable()){
        gps.encode(gpsSerial.getc());  
    //}


}
void radioSerialRecvInterrupt (void)
{
    
    int firstByte, secondByte;
    //get command
    firstByte = radio.getc();  
    if(firstByte == 'c'){
        manual = false;
        secondByte = radio.getc();  
        switch(secondByte){
            case thruster:
                //example command 'ca 50'
                radio.scanf(" %f",thrusterC);
                break;
            case rudder:
                //example command 'cb -20'
                radio.scanf(" %f",rudderC);
                break;
            case combo:
                //example command 'cc -20,50'
                radio.scanf(" %f,%f",rudderC,thrusterC);
                break;
            case control:
                //example command 'cd'
                manual = true;
                break;
            default:
            //shouldn't be here flush stream
                radioFlush();
        }
    }
    else{
        //we shouldnt be here do nothing
        
    }

}

void radioFlush(void) { 
    while (radio.readable()) { 
        radio.getc(); 
    } 
    return; 
}