File content as of revision 23:497f8faa908e:

#include "mbed.h"
#include <string>
#include <sstream>
#include "adapt/usb.h"
#include "handle/handleCamera.h"
#include "handle/handleGPS.h"
#include "mavcontrol.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();
    }
}

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

typedef struct MAV_REQUEST_LIST_S{
    char targSys;
    char targComp;
}MAV_REQUEST_LIST;

typedef struct MAV_COUNT_S{
    uint16_t count;
    char targSys;
    char targComp;
}MAV_COUNT;

typedef struct MAV_REQ_S{
    char targSys;
    char targComp;
    char other;
    char count; 
}MAV_REQ;

typedef struct MAV_APM_S{
    float parm1;
    float parm2;
    float parm3;
    float parm4;
    float parm5;
    float parm6;
    float parm7;
    uint16_t cmd; 
    uint8_t targSys;
    uint8_t targComp;
    uint8_t confirm;
} MAV_APM;

typedef struct MAV_MISSION_ITEM_S{
    float parm1;
    float parm2;
    float parm3;
    float parm4;
    float lat;
    float lon;
    float alt;
    uint16_t seq; 
    uint16_t cmd; 
    uint8_t targSys;
    uint8_t targComp;
    uint8_t frame;
    uint8_t current; // set to true/1 to make current
    uint8_t autoContinue;// set to true/1 to auto continue
    uint8_t confirm;
} MAV_MISSION_ITEM;


int main()
{
    USB::getSerial().printf("Wait 20\n");
    wait(20);
    /*
    char input[9];
    input[0]=0x00;
    input[1]=0x00;
    input[2]=0x00;
    input[3]=0x00;
    input[4]=0x02;
    input[5]=0x03;
    input[6]=0x51;
    input[7]=0x04;
    input[8]=0x03;
    /*
    while(true){
        char input[9];
        input[0]=0x00;
        input[1]=0x00;
        input[2]=0x00;
        input[3]=0x00;
        input[4]=0x02;
        input[5]=0x03;
        input[6]=0x51;
        input[7]=0x04;
        input[8]=0x03;
        Mav::sendOutput(0,input,9);
        wait_ms(50);
    }
    */
    //Mav::sendOutput(MAV_CMD_NAV_TAKEOFF,NULL,0);
    //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
    //Mav::sendOutput(MAVLINK_MSG_ID_LOITERU,NULL,0);
    //Mav::sendOutput(MAV_CMD_NAV_LAND,NULL,0);
    //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0);
    USB::getSerial().printf("System startup...\n");
    char nextCmd[512+1];
    int readIndex=0;
    
    MAV_REQUEST_LIST req;req.targSys=1;req.targComp=1;
    // Issue arm motors
    MAV_APM issueArm;issueArm.targSys=1;issueArm.targComp=250;issueArm.cmd=400;issueArm.parm1=1.0f;
    // Issue disarm motors
    MAV_APM issueDisArm;issueDisArm.targSys=1;issueDisArm.targComp=250;issueDisArm.cmd=400;issueDisArm.parm1=0.0f;
    // Issue a mission count
    MAV_COUNT issueCount;issueCount.targSys=1;issueCount.targComp=1;issueCount.count=1;
    // Issue a mission item
    MAV_MISSION_ITEM issueItem;issueItem.targSys=1;issueItem.targComp=1;issueItem.lat=5.0f;issueItem.lon=6.0f;issueItem.alt=7.0f;issueItem.cmd=16;issueItem.seq=0;issueItem.current=1;
    // Issue a take off item
    MAV_MISSION_ITEM issueTakeOff;issueTakeOff.targSys=1;issueTakeOff.targComp=1;issueTakeOff.lat=5.0f;issueTakeOff.lon=6.0f;issueTakeOff.alt=7.0f;issueTakeOff.cmd=22;issueTakeOff.seq=0;issueTakeOff.current=1;
    
    bool firstRun=true;
    bool gotNewRead=false;
    int readState=0;
    int realLen=0;
    while(true){
        gotNewRead=false;
        if(Mav::getSerial().readable()>0){
            char input=Mav::getSerial().getc();
            USB::getSerial().printf("> %x %d %d\n",input,readState,realLen);
            if(readState==0&&input==0xFE){
                readState=1;
                readIndex=1;
            }else if(readState!=0){
                nextCmd[std::min(readIndex++,512)]=input;
                if(readState==1){
                    realLen=input+5-1;
                    readState=2;
                }
                if(readState==2){
                    realLen--;
                    if(realLen==0){
                        readState=0;
                        gotNewRead=true;
                    }
                }
            }
        }
        // Output debug info
        if(gotNewRead){
            USB::getSerial().printf("Got CMD len %d messageid %d \n",nextCmd[1],nextCmd[5]);
            if(nextCmd[5]==0){
                //Mav::sendOutput(0,input,9);
                //Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
                //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
                if(firstRun){
                    USB::getSerial().printf("Issue Command\n",nextCmd[1],nextCmd[5]);
                    Mav::sendOutput(MAVLINK_MSG_ID_COUNT,(char*)&issueCount,sizeof(MAV_COUNT));
                    //Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
                    //Mav::sendOutput(MAVLINK_MSG_ID_REQUEST_LIST,(char*)&req,sizeof(MAV_REQUEST_LIST));
                }
                firstRun=false;
            }
            if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_REQUEST){
                USB::getSerial().printf("Issue Item\n");
                MAV_REQ* incnt=(MAV_REQ*)(&(nextCmd[6]));
                issueItem.seq=nextCmd[6];//incnt->count;
                USB::getSerial().printf("Got request for %d %d\n",nextCmd[9],incnt->count);
                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueItem,sizeof(MAV_MISSION_ITEM));
            }
            if(nextCmd[5]==MAVLINK_MSG_ID_MISSION_ACK){
                // Arm motors
                Mav::sendOutput(MAVLINK_MSG_ID_LONG,(char*)&issueArm,sizeof(MAV_APM));
                // Take off
                Mav::sendOutput(MAVLINK_MSG_ID_ITEM,(char*)&issueTakeOff,sizeof(MAV_MISSION_ITEM));
            }
        }
    }
    
    int outLength;
    char* output=Mav::generatePacket(MAVLINK_MSG_ID_REQUEST_LIST,NULL,0,&outLength);
    for(int i=0;i<outLength;i++){
        Mav::getSerial().putc(output[i]);
    }
    while(1){}
    
    //getPS().sendPacket(0,NULL,0,PT_EMPTY);getPS().sendPacket(55,NULL,0,PT_IMAGE);
    //char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(0,NULL,0,PT_IMAGE);getPS().sendPacket(55,output,256);getPS().sendPacket(55,output,5);getPS().sendPacket(55,NULL,0,PT_END);
    //while(true){}
 /*   PacketStruct* packs[4];
    for(int i=0;i<4;i){
        packs[i]=getPS().getNextPacket();
        if(packs[i]!=NULL)i++;
    }
    for(int i=0;i<4;i++){
        PacketStruct* pack=packs[i];
        if(pack!=NULL){
            USB::getSerial().printf("Got Packet!\n");
            USB::getSerial().printf(" > %d\n",pack->type);
            for(int i=0;i<sizeof(PacketStruct);i++){
                USB::getSerial().printf("%d\n",((char*)pack)[i]);
            }
            USB::getSerial().printf("\n",pack->type);
        }else{
            //USB::getSerial().printf(".");
        }
    }
    while(true){}
    */
    //getPS().sendPacket(55,NULL,0,PT_IMAGE);char output[256];for(int i=0;i<256;i++){output[i]=i;}getPS().sendPacket(55,output,256,PT_IMAGE);getPS().sendPacket(55,output,5,PT_IMAGE);getPS().sendPacket(55,NULL,0,PT_END);while(true){}
    ImageHandle imageHand;
    GPSHandle gpsHand;
    /// Main Loop
    USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
    //while(1){
        //USB::getSerial().printf("Test\n");
        //XBEE::getSerial().printf("ABC\n");
    //}
    USB::getSerial().printf("Check GPS\n");
    while(1){
        // 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)//;
    }
}