File content as of revision 30:3afafa1ef16b:

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

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_position = 0;
double antennaheight = 200;
int _btMode = 0; // 0=Send Everything, 1 = No GPS, 2 = Receive only
int antenna_active = 0;
//Steering
double lookaheadtime = 2;
double scale = 1;
double phaseadv = 50;
double tcenter = 5;
double fgain = 125;
double avgpos = -4;
// in prevision of PID addition to FreePilot
double kp = 0;
double ki = 0;
double kd = 0;

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;
char tx_line[80];

void Dispatch(char* line, bool config /* = false */)
{
    char* pointer;
    char* Data[5];
    int index = 0;

    bool valid = true;

    //Split data at commas
    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]);

    } else if(strcmp(Data[0], "$TC") == 0) {
        tcenter = atof(Data[1]);

    } else if(strcmp(Data[0], "$FG") == 0) {
        fgain = atof(Data[1]);

    } else if(strcmp(Data[0], "$SC") == 0) {
        scale = atof(Data[1]);

    } else if(strcmp(Data[0], "$AP") == 0) {
        avgpos = atof(Data[1]);

        //}
        //else if(strcmp(Data[0], "$ASTEER") == 0) {
        //    j_guidance = atoi(Data[1]);

    } 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_position = atoi(Data[1]);
    } else if(strcmp(Data[0], "$HEIGHT") == 0) {
        antennaheight = atof(Data[1]);
    } else if(strcmp(Data[0], "$a_zBias") == 0) {
        a_zBias = atof(Data[1]);
    } else if(strcmp(Data[0], "$a_xBias") == 0) {
        a_xBias = atof(Data[1]);
    } else if(strcmp(Data[0], "$a_yBias") == 0) {
        a_yBias = atof(Data[1]);
    } else if(strcmp(Data[0], "$w_zBias") == 0) {
      //  w_zBias = atof(Data[1]);
    } else if(strcmp(Data[0], "$w_xBias") == 0) {
      //  w_xBias = atof(Data[1]);
    } else if(strcmp(Data[0], "$w_yBias") == 0) {
      //  w_yBias = atof(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();
}