File content as of revision 5:7bc8bec23b03:

#include "mbed.h"
#include <string>
#include <sstream>

Serial pc(USBTX,USBRX);
Serial xbee(p9,p10);//tx, rx
Serial gps(p28,p27);

typedef struct {
    int latitude;  //in .0001 minutes
    int longitude; //in .0001 minutes
} GpsCoordinates;

typedef struct {
    int latitude;  //in .0001 minutes
    int longitude; //in .0001 minutes
    int altitude;  //in decimeters
    int time;      //in milliseconds
} GpsData;

GpsData gpsData;
GpsData otherGps;

GpsCoordinates route[256];
uint8_t routeLength;
uint8_t nextWaypoint;
bool routeSet = false;

void readSerial(Serial &s, char str[], int size)
{
    for (int i = 0; i < size; i++) {
        str[i] = s.getc();
    }
}

//sends: "$<command>*<checksum>\r\l"
void sendGpsCommand(string command)
{
    uint8_t checksum = 0;
    pc.printf("Sending command to gps: ");
    gps.putc('$');
    pc.putc('$');
    char c;
    for (int i = 0; i < command.length(); i++) {
        c = command[i];
        checksum ^= c;
        gps.putc(c);
        pc.putc(c);
    }
    gps.putc('*');
    pc.putc('*');
    string checkSumString;
    while (checksum > 0) {
        uint8_t checksumChar = checksum & 0x0F;
        if (checksumChar >= 10) {
            checksumChar -= 10;
            checksumChar += 'A';
        } else {
            checksumChar += '0';
        }
        checkSumString.push_back((char) checksumChar);
        checksum = checksum >> 4;
    }

    for (int i = checkSumString.length() - 1; i >= 0; i--) {
        gps.putc(checkSumString[i]);
        pc.putc(checkSumString[i]);
    }
    gps.putc('\r');
    pc.putc('\r');
    gps.putc('\n');
    pc.putc('\n');
}

long parseDec(string s)
{
    int mult = 1;
    int result = 0;
    for (int i = s.length() - 1; i >=0; i--) {
        if (s[i] != '.') {
            result += (s[i] - '0') * mult;
            mult *= 10;
        }
    }
    return result;
}

//cs: little endian parsing
int nextInt(char *data, int i)
{
    i |= data[i];
    i |= (data[i + 1] << 8);
    i |= (data[i + 2] << 16);
    i |= (data[i + 3] << 24);
    return i;
}

void handleXbeeGps()
{
    static bool reading = false;
    static char packet[16];
    static int i = 0;

    char c = xbee.getc();
    if (reading) {
        packet[i] = c;
        i++;
        if (i == 16) {
            i = 0;
            otherGps.latitude = nextInt(packet, 0);
            otherGps.longitude = nextInt(packet, 4);
            otherGps.altitude = nextInt(packet, 8);
            otherGps.time = nextInt(packet, 12);

            pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
                      otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
                     );
            reading = false;
        }
    } else if (c == 'X') {
        reading = true;
    }
}

void handleGpsData()
{
    static bool reading = false;
    static stringstream line;

    char c = gps.getc();

    if (reading) {
        if (c == '*') { //sentence buffer complete; we're ignoring the checksum
            string field;
            std::getline(line, field, ','); //GPGGA
            std::getline(line, field, ','); //time
            gpsData.time = parseDec(field);
            std::getline(line, field, ','); //latitude
            gpsData.latitude = parseDec(field);
            std::getline(line, field, ','); //N or S
            std::getline(line, field, ','); //longitude
            gpsData.longitude = parseDec(field);
            std::getline(line, field, ','); //E or W
            std::getline(line, field, ','); //skip
            std::getline(line, field, ','); //skip
            std::getline(line, field, ','); //skip
            std::getline(line, field, ','); //altitude
            gpsData.altitude = parseDec(field);

            //update whatever needs updating when gps updates
            pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
                      gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
                     );

            line.str(string(""));
            reading = false;

        } else {
            line.put(c);
        }

    } else if (c == '$') {
        reading = true;
    }
}

uint16_t calcChecksum(char *line, int size){
    //TODO implement
    return 0;    
}

bool setRouteIfValid(char routeLine[])
{
    int lineLen = sizeof(routeLine);

    uint16_t checksum = routeLine[lineLen - 1];
    checksum |= routeLine[lineLen - 2] << 8;
    if (calcChecksum(routeLine, lineLen - 2) != checksum) return false; //checksum invalid

    routeLength = routeLine[0];
    int waypointIndex = 0;
    int lineIndex = 1; //start reading at first coordinate
    for (int i = 0; i < routeLength; i++) {
        route[waypointIndex].latitude = 0;
        route[waypointIndex].longitude = 0;
        route[waypointIndex].latitude |= routeLine[lineIndex];
        route[waypointIndex].latitude |= routeLine[lineIndex + 1] << 8;
        route[waypointIndex].latitude |= routeLine[lineIndex + 2] << 16;
        route[waypointIndex].longitude |= routeLine[lineIndex + 3];
        route[waypointIndex].longitude |= routeLine[lineIndex + 4] << 8;
        route[waypointIndex].longitude |= routeLine[lineIndex + 5] << 16;
        lineIndex += 6;
    }
    return true;
}

//route data format: 'R' numWaypoints   Latitude    Longitude  (repeat numWaypoints times, 6 bytes per waypoint)
// byte indices:      1     2           3  4  5     6  7  8                         (....)
bool handleRouteData(char byte)
{
    if (routeSet) return false; //never allow setting route twice, as can cause bad behavior during flight

    static bool reading = false;
    static char *routeLine;
    static uint8_t numWaypoints;
    static uint8_t i = 0;

    if (!reading) {
        numWaypoints = byte;
        routeLine = (char*) malloc(1 + 6 * numWaypoints + 2); //first byte is number of waypoints, each of which is 6 bytes. Last 2 are checksum
        // route size is kept in line for checksum purposes.
        reading = true;
    }

    routeLine[i] = byte;
    i++;
    if (i == numWaypoints) {
        bool validLine = setRouteIfValid(routeLine);
        if (validLine) routeSet = true;
        free(routeLine);
        reading = false;
        return false;
    } else {
        return true;
    }
}

void handleComm()
{
    static bool reading = false;
    static bool (*handleCommData) (char);
    //cs: handleCommData is a pointer to whatever the current "read communications" function is. It returns a boolean indicating whether
    //      there is more data to be read.
    if (reading) {
        reading = handleCommData(xbee.getc());
    } else {
        switch (xbee.getc()) {
            case 'R':   //route data
                handleCommData = handleRouteData;
                reading = true;
                break;
            default:
                break;
        }
    }
}

int main()
{
    gps.baud(57600);
    xbee.baud(9600);
    pc.baud(57600);

    sendGpsCommand("PMTK301,1");
    while(true) {
        xbee.putc(gps.getc());
    }
//    gps.attach(&handleGpsData, Serial::RxIrq);
//    xbee.attach(&handleXbeeGps, Serial::RxIrq);
}