File content as of revision 28:5905886c76ee:

#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

//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;

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 {
        //bt->printf("Unrecognized config setting detected.\r\n");
        valid = false;
    }
        
   // if(valid && !config)
   //     bt->printf("Command Accepted.");    
}