File content as of revision 7:e8a77af1b428:

#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 thrusterIn(PTD5);
PwmIn rudderIn(PTD0);
PwmOut rudderOut(PTA5);
PwmOut thrusterOut(PTA4);

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

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

int main(void) {
    float tLoop=0,tLoopi=0;
    float rTime=0.00155,tTime=0.00165;
    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.0016);
    thrusterOut.pulsewidth(0.00165);


    t.start();
    while (true) {
        tLoop =t.read();
        if(gps.gga_ready()){
           
            //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
            radio.printf("GPS: %.4f, %.8f, %.8f, %.4f, %.4f\r\n", t.read(),gps.f_lat(), gps.f_lon(),gps.f_course(),gps.f_speed_knots());
        }

        acc.getAccAllAxis(dAcc);
        //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
        radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),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("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),dCompass[0], dCompass[1], dCompass[2]);
        
        while(t.read() - tLoop < 0.5){
            tLoopi=t.read();
            if(manual){               
                //low pass filter the rudder time to stop the jitter
                rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime);
                //if outside limits center the rudder
                if(rTime < 0.001 || rTime > 0.002)rTime=0.00155; 
                tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime);
                //if outside limits thruster to zero
                if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
                
                rudderOut.pulsewidth(rTime);
                thrusterOut.pulsewidth(tTime);
                radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime);
                }
            else{
                //rudderC should be an angle -45 to 45
                //map this to 0.00135 to 0.00175
                rTime=0.0014+0.003*((rudderC+45.0)/90.0);     
                //thrusterC should be an percentage from -100 to 100
                //map this to 0.0011 to 0.0019
                //0.0019 is full reverse
                //0.0011 is full ahead              
                tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0);  
                
                radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);         
                if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;   
                if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
                rudderOut.pulsewidth(rTime);
                
                thrusterOut.pulsewidth(tTime);            
                radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
            }
            //make sure this doesn't run too fast
            if((t.read()-tLoopi)<0.1){
                wait(0.1-(t.read()-tLoopi));
            }
        }
    }
}

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

void gpsSerialRecvInterrupt (void)
{
    

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


}
void radioSerialRecvInterrupt (void)
{
    
    int firstByte=0;
    int command='a';

    if(radio.readable())firstByte=radio.getc();
 
     if(command=='t'){
        
        thrusterC = firstByte-128;
        //limit thrusterC
        if(thrusterC>100){thrusterC=100;}
        if(thrusterC<-100){thrusterC=-100;}
        command=0;
    }
    else if(command=='r'){
        
        rudderC=firstByte-128;
        //limit rudderC
        if(rudderC > 45){rudderC=45;}
        if(rudderC<-45){rudderC=-45;}
        command=0;
    }
 
   if(manual==true){
        //get command
        if(firstByte=='a'){
             manual=false;
             command='a';
        } 
   }else{
        //check if override
        if(firstByte=='b'){
             manual=true;
             command='b';
        }else if(firstByte=='r'){
        //take in 0-255 as -45 to 45 where 128 is 0

            command='r';
            
        }else if(firstByte=='t'){ 

            command=='t';
        }else{
            //do nothing, this will eat all the garbage chars...
        }
        
   
   }
     
   

}

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