Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

packet.h@20:81d5655fecc2, 2014-04-22 (annotated)

Committer:
krobertson
Date:
Tue Apr 22 04:26:31 2014 +0000
Revision:
20:81d5655fecc2
Parent:
19:8c1f2a2204fb
Child:
32:9cb7bc3fc9e0
send and receive commands.; waypoint processing; get locationions; send images

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krobertson 20:81d5655fecc2 1 #ifndef _PACKET_H_
krobertson 20:81d5655fecc2 2 #define _PACKET_H_
krobertson 20:81d5655fecc2 3
krobertson 18:e72ee7aed088 4 #include <InterruptIn.h>
krobertson 20:81d5655fecc2 5 #include "adapt/xbee.h"
krobertson 20:81d5655fecc2 6 #include "adapt/usb.h"
krobertson 20:81d5655fecc2 7 #include "adapt/Timeout.h"
krobertson 18:e72ee7aed088 8
dylanembed123 12:e42985e3ea64 9 #define PACKETSIZE 256
dylanembed123 12:e42985e3ea64 10
dylanembed123 12:e42985e3ea64 11 // Example
dylanembed123 12:e42985e3ea64 12 // Packet 1. (SuperPackid=5,type=PT_IMAGE,size=0)
dylanembed123 12:e42985e3ea64 13 // Packet 2. (SuperPackid=5,type=PT_DEFAULT,size=1024)
dylanembed123 12:e42985e3ea64 14 // Packet 3. (SuperPackid=5,type=PT_DEFAULT,size=1000)
dylanembed123 12:e42985e3ea64 15 // Packet 4. (SuperPackid=5,type=PT_END,size=0)
dylanembed123 15:e3e03a9df89e 16 #define XBEEON
dylanembed123 12:e42985e3ea64 17 enum PACKET_TYPE{
dylanembed123 14:6be57da62283 18 PT_EMPTY=0,
dylanembed123 14:6be57da62283 19 PT_DEFAULT,
dylanembed123 12:e42985e3ea64 20 PT_END,
dylanembed123 12:e42985e3ea64 21 PT_IMAGE,
dylanembed123 15:e3e03a9df89e 22 PT_IMAGEHEAD,
dylanembed123 15:e3e03a9df89e 23 PT_REQLOC,
dylanembed123 15:e3e03a9df89e 24 PT_SENDLOC,
dylanembed123 15:e3e03a9df89e 25 PT_WAY,
krobertson 19:8c1f2a2204fb 26 PT_GETCOMMANDS,
krobertson 20:81d5655fecc2 27 PT_STARTCOMMANDS,
krobertson 20:81d5655fecc2 28 PT_ENDCOMMANDS,
dylanembed123 12:e42985e3ea64 29 PT_SIZE
dylanembed123 12:e42985e3ea64 30 };
dylanembed123 12:e42985e3ea64 31 typedef struct PacketStruct{
dylanembed123 13:a6d3cf2b018e 32 unsigned int type;
dylanembed123 12:e42985e3ea64 33 unsigned int size;// Number of valid bits
dylanembed123 12:e42985e3ea64 34 char data[PACKETSIZE];
dylanembed123 12:e42985e3ea64 35 unsigned int superPackID;//
dylanembed123 14:6be57da62283 36 char special[4];// Set to FF when
dylanembed123 12:e42985e3ea64 37 }PacketStruct;
dylanembed123 12:e42985e3ea64 38
dylanembed123 12:e42985e3ea64 39 class PacketSender{
dylanembed123 12:e42985e3ea64 40 private:
dylanembed123 12:e42985e3ea64 41 unsigned int superID;
dylanembed123 12:e42985e3ea64 42 public:
dylanembed123 12:e42985e3ea64 43 unsigned int getSuperID(){return superID++;}
dylanembed123 13:a6d3cf2b018e 44 PacketSender():superID(0),outputDevice(
dylanembed123 13:a6d3cf2b018e 45 #ifdef XBEEON
dylanembed123 13:a6d3cf2b018e 46 XBEE::getSerial()
dylanembed123 13:a6d3cf2b018e 47 #else
dylanembed123 13:a6d3cf2b018e 48 USB::getSerial()
dylanembed123 13:a6d3cf2b018e 49 #endif
krobertson 18:e72ee7aed088 50 ),setTCPConStatus(
krobertson 18:e72ee7aed088 51 XBEE::getTCPConOut()
krobertson 18:e72ee7aed088 52 ),getTCPConStatus(
krobertson 18:e72ee7aed088 53 XBEE::getTCPConIn()
dylanembed123 16:4f5d20b87dc3 54 ),next(NULL){
dylanembed123 16:4f5d20b87dc3 55 //outputDevice.attach(this,&PacketSender::handleUpdate,Serial::RxIrq);
dylanembed123 16:4f5d20b87dc3 56 lastValid=NULL;
dylanembed123 16:4f5d20b87dc3 57 }
dylanembed123 12:e42985e3ea64 58 Serial& outputDevice;
krobertson 18:e72ee7aed088 59 DigitalOut& setTCPConStatus;
krobertson 18:e72ee7aed088 60 DigitalIn& getTCPConStatus;
dylanembed123 12:e42985e3ea64 61 void sendPacket(PacketStruct& output){
dylanembed123 12:e42985e3ea64 62 for(int a=0;a<sizeof(PacketStruct);a++){
dylanembed123 14:6be57da62283 63 while(!outputDevice.writeable()){}
dylanembed123 12:e42985e3ea64 64 outputDevice.putc(((char*)(&output))[a]);
dylanembed123 15:e3e03a9df89e 65 //wait_us(10);
dylanembed123 13:a6d3cf2b018e 66 //USB::getSerial().putc(((char*)(&output))[a]);
dylanembed123 12:e42985e3ea64 67 }
dylanembed123 14:6be57da62283 68 //wait_ms(100);
dylanembed123 12:e42985e3ea64 69 }
krobertson 18:e72ee7aed088 70
krobertson 18:e72ee7aed088 71 void openConnection(char close_conn = 0){
krobertson 19:8c1f2a2204fb 72 do{
krobertson 19:8c1f2a2204fb 73 USB::getSerial().printf("trying to connect...\r\n");
krobertson 19:8c1f2a2204fb 74 if(getTCPConStatus){
krobertson 19:8c1f2a2204fb 75 setTCPConStatus = 0;
krobertson 19:8c1f2a2204fb 76 while(getTCPConStatus){}
krobertson 19:8c1f2a2204fb 77 wait_us(50000);
krobertson 19:8c1f2a2204fb 78 }
krobertson 18:e72ee7aed088 79 setTCPConStatus = 1;
krobertson 19:8c1f2a2204fb 80 while(setTCPConStatus==1 && !getTCPConStatus){}
krobertson 19:8c1f2a2204fb 81 }while(!getTCPConStatus);
krobertson 18:e72ee7aed088 82 }
krobertson 20:81d5655fecc2 83
krobertson 18:e72ee7aed088 84 void closeConnection(){
krobertson 19:8c1f2a2204fb 85 wait_us(50000);
krobertson 19:8c1f2a2204fb 86 do{
krobertson 19:8c1f2a2204fb 87 USB::getSerial().printf("disconnecting...\r\n");
krobertson 18:e72ee7aed088 88 setTCPConStatus = 0;
krobertson 18:e72ee7aed088 89 wait_us(50000);
krobertson 19:8c1f2a2204fb 90 }while(getTCPConStatus);
krobertson 18:e72ee7aed088 91 }
krobertson 20:81d5655fecc2 92
krobertson 20:81d5655fecc2 93 void clearRXBuffer(){
krobertson 20:81d5655fecc2 94 while(outputDevice.readable()){
krobertson 20:81d5655fecc2 95 outputDevice.getc();
krobertson 20:81d5655fecc2 96 }
krobertson 20:81d5655fecc2 97 }
krobertson 20:81d5655fecc2 98
krobertson 20:81d5655fecc2 99 char rx_ready_with_timeout(){
krobertson 20:81d5655fecc2 100 if(outputDevice.readable()){
krobertson 20:81d5655fecc2 101 return 1;
krobertson 20:81d5655fecc2 102 }else{
krobertson 20:81d5655fecc2 103 EvTimer t;
krobertson 20:81d5655fecc2 104 t.set_s_period(3.0);
krobertson 20:81d5655fecc2 105 t.start_timer();
krobertson 20:81d5655fecc2 106 while(t.get_num_trips() == 0){
krobertson 20:81d5655fecc2 107 if(outputDevice.readable()){
krobertson 20:81d5655fecc2 108 return 1;
krobertson 20:81d5655fecc2 109 }
krobertson 20:81d5655fecc2 110 }
krobertson 20:81d5655fecc2 111 t.stop_timer();
krobertson 20:81d5655fecc2 112 }
krobertson 20:81d5655fecc2 113 return 0;
krobertson 20:81d5655fecc2 114 }
krobertson 20:81d5655fecc2 115
krobertson 20:81d5655fecc2 116 char getSynced(){
krobertson 20:81d5655fecc2 117 int zero_count = 0; //count of the number of consecutive 0's
krobertson 20:81d5655fecc2 118 char sel_ret;
krobertson 20:81d5655fecc2 119 unsigned char temp;
krobertson 20:81d5655fecc2 120 while(1){
krobertson 20:81d5655fecc2 121 if((sel_ret = rx_ready_with_timeout()) != 1){
krobertson 20:81d5655fecc2 122 return sel_ret;
krobertson 20:81d5655fecc2 123 }
krobertson 20:81d5655fecc2 124 temp = outputDevice.getc();
krobertson 20:81d5655fecc2 125 //USB::getSerial().printf("%x ",temp);
krobertson 20:81d5655fecc2 126 if(temp==0){
krobertson 20:81d5655fecc2 127 zero_count++;
krobertson 20:81d5655fecc2 128 }else if(temp == 0xFF){
krobertson 20:81d5655fecc2 129 printf("TESTING SYNC COMPLETE: %d. expecting %d\n",zero_count,sizeof(PacketStruct)-sizeof(char));
krobertson 20:81d5655fecc2 130 if(zero_count == sizeof(PacketStruct)-sizeof(char)){
krobertson 20:81d5655fecc2 131 USB::getSerial().printf("!!!!DONE SYNCING!!!!\r\n");
krobertson 20:81d5655fecc2 132 return 1;
krobertson 20:81d5655fecc2 133 }
krobertson 20:81d5655fecc2 134 }else{
krobertson 20:81d5655fecc2 135 zero_count = 0;
krobertson 20:81d5655fecc2 136 }
krobertson 20:81d5655fecc2 137 if(zero_count > sizeof(PacketStruct)-sizeof(char)){
krobertson 20:81d5655fecc2 138 zero_count = 0;
krobertson 20:81d5655fecc2 139 }
krobertson 20:81d5655fecc2 140 }
krobertson 20:81d5655fecc2 141 }
krobertson 20:81d5655fecc2 142
krobertson 20:81d5655fecc2 143 char receivePacket(PacketStruct* pack){
krobertson 20:81d5655fecc2 144 char rx_ret;
krobertson 20:81d5655fecc2 145 char temp;
krobertson 20:81d5655fecc2 146 int total_rec_bytes = 0;//total bytes received for this packet.
krobertson 20:81d5655fecc2 147 while(total_rec_bytes < sizeof(PacketStruct)){
krobertson 20:81d5655fecc2 148 if((rx_ret = rx_ready_with_timeout()) != 1){
krobertson 20:81d5655fecc2 149 USB::getSerial().printf("timed out waiting for packet. Received %d/%d bytes\r\n",total_rec_bytes,sizeof(PacketStruct));
krobertson 20:81d5655fecc2 150 return rx_ret;
krobertson 20:81d5655fecc2 151 }
krobertson 20:81d5655fecc2 152 temp = outputDevice.getc();
krobertson 20:81d5655fecc2 153 *(((char*)pack) + total_rec_bytes) = temp;
krobertson 20:81d5655fecc2 154 total_rec_bytes++;
krobertson 20:81d5655fecc2 155 }
krobertson 20:81d5655fecc2 156 return 1;
krobertson 20:81d5655fecc2 157 }
krobertson 20:81d5655fecc2 158
dylanembed123 12:e42985e3ea64 159 unsigned int min(unsigned int a,unsigned int b){
dylanembed123 12:e42985e3ea64 160 return a<b ? a : b;
dylanembed123 12:e42985e3ea64 161 }
krobertson 20:81d5655fecc2 162
dylanembed123 12:e42985e3ea64 163 void sendPacket(unsigned int superPackID,char* data,unsigned int size,PACKET_TYPE type = PT_END){
dylanembed123 12:e42985e3ea64 164 if(data!=NULL && size>0){
dylanembed123 13:a6d3cf2b018e 165 for(int i=0;i<=(size-1)/PACKETSIZE;i++){
dylanembed123 12:e42985e3ea64 166 PacketStruct output;
dylanembed123 12:e42985e3ea64 167 output.type=PT_DEFAULT;
dylanembed123 12:e42985e3ea64 168 output.superPackID=superPackID;
dylanembed123 12:e42985e3ea64 169 output.size=min(PACKETSIZE,size-i*PACKETSIZE);
dylanembed123 14:6be57da62283 170 for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
dylanembed123 14:6be57da62283 171 for(int a=0;a<output.size;a++){output.data[a]=data[a-i*PACKETSIZE];}
dylanembed123 13:a6d3cf2b018e 172 for(int a=output.size;a<PACKETSIZE;a++){output.data[a]='\0';}
dylanembed123 12:e42985e3ea64 173 sendPacket(output);
dylanembed123 12:e42985e3ea64 174 }
dylanembed123 12:e42985e3ea64 175 }else{
dylanembed123 12:e42985e3ea64 176 PacketStruct output;
dylanembed123 12:e42985e3ea64 177 output.type=type;
dylanembed123 13:a6d3cf2b018e 178 output.size=0;
dylanembed123 12:e42985e3ea64 179 output.superPackID=superPackID;
dylanembed123 14:6be57da62283 180 for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
dylanembed123 14:6be57da62283 181 // Check for empty packet
dylanembed123 14:6be57da62283 182 if(output.type==PT_EMPTY){output.type=0;output.size=0;output.superPackID=0;output.special[3]=0xFF;}
dylanembed123 14:6be57da62283 183 for(int a=0;a<PACKETSIZE;a++){output.data[a]='\0';}
dylanembed123 12:e42985e3ea64 184 sendPacket(output);
dylanembed123 12:e42985e3ea64 185 }
dylanembed123 12:e42985e3ea64 186 }
krobertson 20:81d5655fecc2 187
dylanembed123 16:4f5d20b87dc3 188 PacketStruct* lastValid;
dylanembed123 16:4f5d20b87dc3 189 bool found;
dylanembed123 16:4f5d20b87dc3 190 void handleUpdate(){
dylanembed123 16:4f5d20b87dc3 191 USB::getSerial().printf("Interupt\n");
dylanembed123 16:4f5d20b87dc3 192 PacketStruct* next = getNextPacket();
dylanembed123 16:4f5d20b87dc3 193 if(next!=NULL){lastValid=next;}
dylanembed123 16:4f5d20b87dc3 194 }
dylanembed123 14:6be57da62283 195
dylanembed123 14:6be57da62283 196 // Number of consecutive zeros
dylanembed123 14:6be57da62283 197 unsigned int numZeros;
dylanembed123 14:6be57da62283 198 // Return true if a resync command has been received
dylanembed123 14:6be57da62283 199 bool resetCheck(char input){
dylanembed123 14:6be57da62283 200 if(input=='\0'){
dylanembed123 14:6be57da62283 201 numZeros++;
dylanembed123 14:6be57da62283 202 }else if(numZeros==sizeof(PacketStruct)-1&&input==0xFF){
dylanembed123 14:6be57da62283 203 return true;
dylanembed123 14:6be57da62283 204 }else{
dylanembed123 14:6be57da62283 205 numZeros=0;
dylanembed123 14:6be57da62283 206 }
dylanembed123 14:6be57da62283 207 return false;
dylanembed123 14:6be57da62283 208 }
dylanembed123 14:6be57da62283 209
dylanembed123 14:6be57da62283 210 // Temperary storage for next valid
dylanembed123 14:6be57da62283 211 PacketStruct* next;
dylanembed123 14:6be57da62283 212 // Number of valid bits in next packet
dylanembed123 14:6be57da62283 213 int nextValid;
dylanembed123 14:6be57da62283 214 /// \brief Grab the next packet
dylanembed123 12:e42985e3ea64 215 PacketStruct* getNextPacket(){
dylanembed123 14:6be57da62283 216 // Check for null packet
dylanembed123 14:6be57da62283 217 if(next==NULL){next=new PacketStruct();nextValid=0;}
dylanembed123 14:6be57da62283 218 // Create reset packet which resets on re-sync command
dylanembed123 14:6be57da62283 219 bool resetPacket=false;
dylanembed123 14:6be57da62283 220
dylanembed123 14:6be57da62283 221 // While there is data to read
dylanembed123 14:6be57da62283 222 while(0<outputDevice.readable()){
dylanembed123 14:6be57da62283 223 // Check if a full packet has been received
dylanembed123 14:6be57da62283 224 if(nextValid==sizeof(PacketStruct))break;
dylanembed123 14:6be57da62283 225 // Read in next char
dylanembed123 14:6be57da62283 226 char input=outputDevice.getc();
dylanembed123 16:4f5d20b87dc3 227 USB::getSerial().printf("Read ByteC %X %X\n",nextValid,input);
dylanembed123 14:6be57da62283 228 // Check for valid char
dylanembed123 14:6be57da62283 229 if(resetCheck(input)){resetPacket=true;break;}
dylanembed123 14:6be57da62283 230 // Set char
dylanembed123 14:6be57da62283 231 ((char*)next)[nextValid++] = input;
dylanembed123 14:6be57da62283 232 }
dylanembed123 14:6be57da62283 233
dylanembed123 14:6be57da62283 234 if(nextValid==sizeof(PacketStruct)||resetPacket){
dylanembed123 14:6be57da62283 235 // Reset packet
dylanembed123 14:6be57da62283 236 PacketStruct* output=next;next=NULL;
dylanembed123 14:6be57da62283 237 // Return
dylanembed123 14:6be57da62283 238 return resetPacket?NULL:output;
dylanembed123 14:6be57da62283 239 }
dylanembed123 14:6be57da62283 240 return NULL;
dylanembed123 14:6be57da62283 241 /*
dylanembed123 12:e42985e3ea64 242 int avail = outputDevice.readable();
dylanembed123 12:e42985e3ea64 243 if(avail <= 0)return NULL;
dylanembed123 12:e42985e3ea64 244 PacketStruct* output=new PacketStruct();
dylanembed123 12:e42985e3ea64 245 for(int i=0;i<sizeof(PacketStruct);i++){
dylanembed123 12:e42985e3ea64 246 // Wait for byte
dylanembed123 12:e42985e3ea64 247 while(outputDevice.readable()<=0){}
dylanembed123 12:e42985e3ea64 248 ((char*)output)[i] = outputDevice.getc();
dylanembed123 12:e42985e3ea64 249 }
dylanembed123 12:e42985e3ea64 250 return output;
dylanembed123 14:6be57da62283 251 */
dylanembed123 12:e42985e3ea64 252 }
dylanembed123 12:e42985e3ea64 253
dylanembed123 12:e42985e3ea64 254 };
dylanembed123 12:e42985e3ea64 255 static PacketSender* ps=NULL;
dylanembed123 12:e42985e3ea64 256 static PacketSender& getPS(){
dylanembed123 12:e42985e3ea64 257 if(ps==NULL)ps=new PacketSender();
dylanembed123 12:e42985e3ea64 258 return *ps;
dylanembed123 12:e42985e3ea64 259 }
krobertson 20:81d5655fecc2 260
krobertson 20:81d5655fecc2 261 #endif