File content as of revision 25:b7f861fc8ddd:

#include "mbed.h"
#include <string>
#include <sstream>
#include "adapt/usb.h"
#include "handle/handleCamera.h"
#include "handle/handleGPS.h"
#include "handle/handleCommand.h"
#include "mavcommands.h"

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

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

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

void connection_lost(){
    USB::getSerial().printf("TCP connection lost!\r\n");    
}

//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');
}
//
////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;
//    }
//}

int main()
{

    // Start Mav test
    /*
    USB::getSerial().printf("Wait 20\n");
    wait(20);    
    while(true){
        MavCmd::get().run();
    }
    */
    // End mav test
    
    //handlers
    //ImageHandle imageHand;
    //GPSHandle gpsHand;
    //CommandHandle commHand;

    USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
    USB::getSerial().printf("Check GPS\n");
    USB::getSerial().printf("Connect to the wifly network now!\r\n");
    //XBEE::getTCPInterrupt().fall(&connection_lost);
    //checking connection to egg before continuing
    getPS().openConnection();
    getPS().closeConnection();
    //Main Loop
        while(1){
            USB::getSerial().printf("Requesting commands from egg...\r\n");
            wait_us(100000);
            CommandHandle::getCommandHand().run();
            wait_us(100000);
            if(GPSHandle::getGPSHand().if_image_location()){
                USB::getSerial().printf("Taking picture and sending...\r\n");
                wait_us(100000);
                ImageHandle::getImageHand().run();
                USB::getSerial().printf("sent all data\r\n");
                wait_us(100000);
            }
            wait_us(1000000);
        //}

        // Run image handler
        //USB::getSerial().printf("Check Image\n");
       // imageHand.run();
        // Run GPS handler
       // USB::getSerial().printf("Check GPS\n");
        //gpsHand.run();
        //USB::getSerial().printf("GPS Time: %f\n",DH::Locs().getC().getTime());
        // Read packet
        //USB::getSerial().printf("Read Input\n");
        //PacketStruct* pack=getPS().lastValid;//getPS().getNextPacket();
        //if(pack!=NULL){
        //    USB::getSerial().printf("Received Type: %d\n",pack->type);
        //    if(pack->type==PT_REQLOC){
        //if(getPS().outputDevice.readable()>0){
//                char input=getPS().outputDevice.getc();
//                //if(getPS().outputDevice.getc()=='A'){
//                // Send location
//                unsigned int sID=getPS().getSuperID();
//                getPS().sendPacket(0,NULL,0,PT_EMPTY);
//                getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
//                getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation));
//                getPS().sendPacket(sID,NULL,0,PT_END);
//                //}
//        }
    }
    /// Main Loop
   // while(true) {
//        gps.baud(57600);
//        xbee.baud(9600);
//        pc.baud(57600);
//
//        sendGpsCommand("PMTK301,1");
//        while(true) {
//            pc.putc(gps.getc());
//        }
        //gps.attach(&handleGpsData, Serial::RxIrq);
        //xbee.attach(&handleXbeeGps, Serial::RxIrq)//;
    //}
}