File content as of revision 66:5d43988d100c:

#ifndef _PACKET_H_
#define _PACKET_H_

#include <InterruptIn.h>
#include "adapt/xbee.h"
#include "adapt/usb.h"
#include "adapt/Timeout.h"

#define PACKETSIZE 256

// Example
// Packet 1. (SuperPackid=5,type=PT_IMAGE,size=0)
// Packet 2. (SuperPackid=5,type=PT_DEFAULT,size=1024)
// Packet 3. (SuperPackid=5,type=PT_DEFAULT,size=1000)
// Packet 4. (SuperPackid=5,type=PT_END,size=0)
#define XBEEON
enum PACKET_TYPE{
    PT_EMPTY=0,
    PT_DEFAULT,
    PT_END,
    PT_IMAGE,
    PT_IMAGEHEAD,
    PT_REQLOC,
    PT_SENDLOC,
    PT_WAY,
    PT_GETCOMMANDS,
    PT_STARTCOMMANDS,
    PT_ENDCOMMANDS,
    PT_SIZE
};
typedef struct PacketStruct{
    unsigned int type;
    unsigned int size;// Number of valid bits
    char data[PACKETSIZE];
    unsigned int superPackID;// 
    char special[4];// Set to FF when
}PacketStruct;

class PacketSender{
    private:
    unsigned int superID;
    public:
    unsigned int getSuperID(){return superID++;}
    PacketSender():superID(0),outputDevice(
        #ifdef XBEEON
        XBEE::getSerial()
        #else
        USB::getSerial()
        #endif
    ),setTCPConStatus(
        XBEE::getTCPConOut()
    ),getTCPConStatus(
        XBEE::getTCPConIn()
    ),next(NULL){
        //outputDevice.attach(this,&PacketSender::handleUpdate,Serial::RxIrq);
        lastValid=NULL;
    }
    Serial& outputDevice;
    DigitalOut& setTCPConStatus;
    DigitalIn& getTCPConStatus;
    void sendPacket(PacketStruct& output){
        for(int a=0;a<sizeof(PacketStruct);a++){
            while(!outputDevice.writeable()){}
            outputDevice.putc(((char*)(&output))[a]);
            //wait_us(10);
            //USB::getSerial().putc(((char*)(&output))[a]);
        }
        //wait_ms(100);
    }
    
    void openConnection(char close_conn = 0, char hover_attempt = 0){
        EvTimer t;
        t.set_s_period(30);
        t.start_timer();
        char con_status_steady;
        char timed_out = 0;
        do{
            USB::getSerial().printf("trying to connect...\r\n");
            if(getTCPConStatus){
                setTCPConStatus = 0;
                while(getTCPConStatus){}
                wait_us(200000);
            }
            setTCPConStatus = 1;
            con_status_steady = 1;
            wait_us(200000);
            for(int i=0;i<10;i++){
                if(!getTCPConStatus){
                    con_status_steady = 0;
                    break;
                }
                wait_us(1000);
            }
            timed_out = t.get_num_trips();
        }while(!con_status_steady && !timed_out);
        t.stop_timer();
        if(timed_out>0){
            if(hover_attempt){
                //emergency landing goes here
                USB::getSerial().printf("Second Attempt Connection failure. Emergency Landing.\r\n");
                wait_us(10000000);
            }else{
                //hover and give it another shot
                USB::getSerial().printf("First Attempt Connection failure. Hover and retry.\r\n");
                //hover code goes here
                wait_us(10000000);
                openConnection(close_conn, 1);
            }
        }
    }
    
    void closeConnection(){
        wait_us(50000);
        do{
            USB::getSerial().printf("disconnecting...\r\n");
            setTCPConStatus = 0;
            wait_us(50000);
        }while(getTCPConStatus);
    }
    
    void clearRXBuffer(){
        while(outputDevice.readable()){
            outputDevice.getc();
        }
    }
    
    char rx_ready_with_timeout(Serial* serialDevice = NULL, float seconds = 0, unsigned int u_seconds = 0){
        if(serialDevice == NULL){
            serialDevice = &outputDevice;
        }
        if(serialDevice->readable()){
            return 1;
        }else{
            EvTimer t;
             if(seconds==0 && u_seconds==0){
                seconds = 3.0;
            }
            if(seconds>0){
                t.set_s_period(seconds);
            }else{
                t.set_us_period(u_seconds);
            }
            t.start_timer();
            while(t.get_num_trips() == 0){
                if(serialDevice->readable()){
                    t.stop_timer();
                    return 1;
                }
            }
            t.stop_timer();
        }
        return 0;
    }
    
    char getSynced(){
        int zero_count = 0; //count of the number of consecutive 0's
        char sel_ret;
        unsigned char temp;
        while(1){
            if((sel_ret = rx_ready_with_timeout()) != 1){
                return sel_ret;
            }
            temp = outputDevice.getc();
            //USB::getSerial().printf("%x ",temp);
            if(temp==0){
                zero_count++;
            }else if(temp == 0xFF){
                printf("TESTING SYNC COMPLETE: %d. expecting %d\n",zero_count,sizeof(PacketStruct)-sizeof(char));
                if(zero_count == sizeof(PacketStruct)-sizeof(char)){
                    USB::getSerial().printf("!!!!DONE SYNCING!!!!\r\n");
                    return 1;
                }
            }else{
                zero_count = 0;
            }
            if(zero_count > sizeof(PacketStruct)-sizeof(char)){
                zero_count = 0;
            }
        }
    }
    
    char receivePacket(PacketStruct* pack){
        char rx_ret;
        char temp;
        int total_rec_bytes = 0;//total bytes received for this packet.
        while(total_rec_bytes < sizeof(PacketStruct)){
            if(!outputDevice.readable() && (rx_ret = rx_ready_with_timeout(&outputDevice, 3)) != 1){
                USB::getSerial().printf("timed out waiting for packet. Received %d/%d bytes\r\n",total_rec_bytes,sizeof(PacketStruct));
                return rx_ret;
            }
            temp = outputDevice.getc();
            //USB::getSerial().printf("%c ",temp);
            *(((char*)pack) + total_rec_bytes) = temp;
            total_rec_bytes++;
        }
        return 1;
    }
    
    unsigned int min(unsigned int a,unsigned int b){
        return a<b ? a : b;
    }
    
    void sendPacket(unsigned int superPackID,char* data,unsigned int size,PACKET_TYPE type = PT_END){
        if(data!=NULL && size>0){
            for(int i=0;i<=(size-1)/PACKETSIZE;i++){
                PacketStruct output;
                output.type=PT_DEFAULT;
                output.superPackID=superPackID;
                output.size=min(PACKETSIZE,size-i*PACKETSIZE);
                for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
                for(int a=0;a<output.size;a++){output.data[a]=data[a-i*PACKETSIZE];}
                for(int a=output.size;a<PACKETSIZE;a++){output.data[a]='\0';}
                sendPacket(output);
            }
        }else{
            PacketStruct output;
            output.type=type;
            output.size=0;
            output.superPackID=superPackID;
            for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
            // Check for empty packet
            if(output.type==PT_EMPTY){output.type=0;output.size=0;output.superPackID=0;output.special[3]=0xFF;}
            for(int a=0;a<PACKETSIZE;a++){output.data[a]='\0';}
            sendPacket(output);
        }
    }
    
    PacketStruct* lastValid;
    bool found;
    void handleUpdate(){
        USB::getSerial().printf("Interupt\n");
        PacketStruct* next = getNextPacket();
        if(next!=NULL){lastValid=next;}
    }
    
    // Number of consecutive zeros
    unsigned int numZeros;
    // Return true if a resync command has been received
    bool resetCheck(char input){
        if(input=='\0'){
            numZeros++;
        }else if(numZeros==sizeof(PacketStruct)-1&&input==0xFF){
            return true;
        }else{
            numZeros=0;
        }
        return false;
    }
    
    // Temperary storage for next valid
    PacketStruct* next;
    // Number of valid bits in next packet
    int nextValid;
    /// \brief Grab the next packet
    PacketStruct* getNextPacket(){
        // Check for null packet
        if(next==NULL){next=new PacketStruct();nextValid=0;}
        // Create reset packet which resets on re-sync command
        bool resetPacket=false;

        // While there is data to read
        while(0<outputDevice.readable()){
            // Check if a full packet has been received
            if(nextValid==sizeof(PacketStruct))break;
            // Read in next char
            char input=outputDevice.getc();
            USB::getSerial().printf("Read ByteC %X %X\n",nextValid,input);
            // Check for valid char
            if(resetCheck(input)){resetPacket=true;break;}
            // Set char
            ((char*)next)[nextValid++] = input;
        }
        
        if(nextValid==sizeof(PacketStruct)||resetPacket){
            // Reset packet
            PacketStruct* output=next;next=NULL;
            // Return
            return resetPacket?NULL:output;
        }
        return NULL;
/*        
        int avail = outputDevice.readable();
        if(avail <= 0)return NULL;
        PacketStruct* output=new PacketStruct();
        for(int i=0;i<sizeof(PacketStruct);i++){
            // Wait for byte
            while(outputDevice.readable()<=0){}
            ((char*)output)[i] = outputDevice.getc();
        }
        return output;
*/
    }
    
};
static PacketSender* ps=NULL;
static PacketSender& getPS(){
    if(ps==NULL)ps=new PacketSender();
    return *ps;
}

#endif