File content as of revision 46:d7d6dc429153:

#include "mbed.h"
#include "MODSERIAL.h"
#include <string>
#include "base.h"
#include "Config.h"

const int debug=1; //set to 0 to disable output to USB, set to 1 to output data to USB

DigitalOut ledGREEN(p11);
DigitalOut ledRED(p12);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

extern "C" void mbed_mac_address(char *s);
int _ID = 0; //stores this mbed's ID

// Connect the TX of the GPS module to p10 RX input
MODSERIAL gps(p9, p10); //GPS
MODSERIAL pc(USBTX, USBRX);
MODSERIAL bluetooth(p13, p14);

// GLOBAL VALUES ONLY=====================
// values which are stored in config.txt         //
int gyro_pos = 0;
double antennaheight = 78.74;
int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
int antenna_active = 0;
//Steering
double lookaheadtime = 4;
double scale = 0.5;
double phaseadv = 40;
double tcenter = 4.6;
double fgain = 22;
double avgpos = 4;
// in prevision of PID addition to FreePilot
double kp = 3;
double ki = 2;
double kd = 1;

int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.

//offsets
double w_xBias;
double w_yBias;
double w_zBias;
double a_xBias;
double a_yBias;
double a_zBias;

bool Authenticated = 0;
bool freepilot_v2 = true;
char tx_line[80];

void Dispatch(char* line, bool config /* = false */)
{
    char* pointer;
    char* Data[5];
    int index = 0;
    bool valid = true;
    pointer = strtok(line, ",");
    if(pointer == NULL)
        Data[0] = line;
    while(pointer != NULL) {
        Data[index] = pointer;
        pointer = strtok(NULL, ",");
        index++;
    }
    //Check ID of read data and set the corresponding variable.
    if(strcmp(Data[0], "$ID") == 0) 
    {
        //_ID = atoi(Data[1]);
      //  if(Config_GetID() == _ID) 
       // {
            // bt->printf("Board ID Matches.\r\n");
            Authenticated = true;
       // } 
       // else 
       // {
       //     Authenticated = false;
            //  bt->printf("Board ID does not match.\r\n");
      //  }
    } 
    else if(strcmp(Data[0], "$BANY") == 0) 
    {
        // if(!Authenticated)
        //    RestartRequired = true;
        _ID = Config_GetID();
        Config_Save();
        // bt->printf("Adress set: %d \r\n", _ID);
    } 
    else if(strcmp(Data[0], "$PA") == 0) 
    {
        phaseadv = atof(Data[1]);
       // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
    } 
    else if(strcmp(Data[0], "$TC") == 0) 
    {
        tcenter = atof(Data[1]);
       // SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
    }   
    else if(strcmp(Data[0], "$V2,1") == 0) 
    {
        freepilot_v2 = true;
    } 
        else if(strcmp(Data[0], "$V2,0") == 0) 
    {
        freepilot_v2 = false;
    } 
    else if(strcmp(Data[0], "$FG") == 0) 
    {
        fgain = atof(Data[1]);
      //  SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
    } 
    else if(strcmp(Data[0], "$SC") == 0) 
    {
        scale = atof(Data[1]);
      //  SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
    } 
    else if(strcmp(Data[0], "$AP") == 0) 
    {
        avgpos = atof(Data[1]);
    } 
    else if(strcmp(Data[0], "$LA") == 0) 
    {
        lookaheadtime = atof(Data[1]);
      //  SetFilter(phaseadv,tcenter, lookaheadtime, scale, fgain );
    }
    else if(strcmp(Data[0], "$PCALIVE") == 0) 
    {
        //pc connection watchdog
    } 
    else if(strcmp(Data[0], "$SAVE") == 0) 
    {
        Config_Save();
    } 
    else if(strcmp(Data[0], "$BTMODE") == 0) 
    {
        _btMode = atoi(Data[1]);
    } 
    else if (strcmp(Data[0],"$GPSBAUD") == 0 ) 
    {
        gps_baud = atoi(Data[1]);
        activate_antenna();
    } 
    else if(strcmp(Data[0], "$GYRO") == 0) 
    {
        gyro_pos = atoi(Data[1]);
    } 
    else if(strcmp(Data[0], "$HEIGHT") == 0) 
    {
        antennaheight = atof(Data[1]);
    }
    else if(strcmp(Data[0], "$POSITION") == 0) 
    {
        gyro_pos = atoi(Data[1]);
    } 
    else 
    {
        //bt->printf("Unrecognized config setting detected.\r\n");
        valid = false;
    }
    // if(valid && !config)
    //     bt->printf("Command Accepted.");
}

void activate_antenna()
{
    gps.baud(gps_baud);
    antenna_active = 1;
}

void process_GPSBAUD(char* gpsbaud)
{
    char *token;
    int  token_counter = 0;
    char *baud  = (char *)NULL;
    token = strtok(gpsbaud, ",");
    while (token) 
    {
        switch (token_counter) 
        {
            case 1:
                baud = token;
                break;
        }
        token = strtok((char *)NULL, ",");
        token_counter++;
    }
    if ( baud ) 
    {
        gps_baud = atoi(baud);
    }
    activate_antenna();
}