Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

packet.h@45:0d6ef4cbd4c7, 2014-04-24 (annotated)

Committer:
krobertson
Date:
Thu Apr 24 16:16:57 2014 +0000
Revision:
45:0d6ef4cbd4c7
Parent:
41:df156ae5631b
Child:
46:f89fc45811a1
connection timeouts added

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 45:0d6ef4cbd4c7 71 void openConnection(char close_conn = 0, char hover_attempt = 0){
krobertson 45:0d6ef4cbd4c7 72 EvTimer t;
krobertson 45:0d6ef4cbd4c7 73 t.set_s_period(30);
krobertson 45:0d6ef4cbd4c7 74 t.start_timer();
krobertson 45:0d6ef4cbd4c7 75 char timed_out = 0;
krobertson 19:8c1f2a2204fb 76 do{
krobertson 19:8c1f2a2204fb 77 USB::getSerial().printf("trying to connect...\r\n");
krobertson 19:8c1f2a2204fb 78 if(getTCPConStatus){
krobertson 19:8c1f2a2204fb 79 setTCPConStatus = 0;
krobertson 19:8c1f2a2204fb 80 while(getTCPConStatus){}
krobertson 32:9cb7bc3fc9e0 81 wait_us(100000);
krobertson 19:8c1f2a2204fb 82 }
krobertson 18:e72ee7aed088 83 setTCPConStatus = 1;
krobertson 32:9cb7bc3fc9e0 84 wait_us(100000);
krobertson 45:0d6ef4cbd4c7 85 timed_out = t.get_num_trips();
krobertson 45:0d6ef4cbd4c7 86 }while(!getTCPConStatus && !timed_out);
krobertson 45:0d6ef4cbd4c7 87 t.stop_timer();
krobertson 45:0d6ef4cbd4c7 88 if(timed_out>0){
krobertson 45:0d6ef4cbd4c7 89 if(hover_attempt){
krobertson 45:0d6ef4cbd4c7 90 //emergency landing goes here
krobertson 45:0d6ef4cbd4c7 91 USB::getSerial().printf("Second Attempt Connection failure. Emergency Landing.\r\n");
krobertson 45:0d6ef4cbd4c7 92 }else{
krobertson 45:0d6ef4cbd4c7 93 //hover and give it another shot
krobertson 45:0d6ef4cbd4c7 94 USB::getSerial().printf("First Attempt Connection failure. Hover and retry.\r\n");
krobertson 45:0d6ef4cbd4c7 95 //hover code goes here
krobertson 45:0d6ef4cbd4c7 96 openConnection(close_conn, 1);
krobertson 45:0d6ef4cbd4c7 97 }
krobertson 45:0d6ef4cbd4c7 98 }
krobertson 18:e72ee7aed088 99 }
krobertson 20:81d5655fecc2 100
krobertson 18:e72ee7aed088 101 void closeConnection(){
krobertson 19:8c1f2a2204fb 102 wait_us(50000);
krobertson 19:8c1f2a2204fb 103 do{
krobertson 19:8c1f2a2204fb 104 USB::getSerial().printf("disconnecting...\r\n");
krobertson 18:e72ee7aed088 105 setTCPConStatus = 0;
krobertson 18:e72ee7aed088 106 wait_us(50000);
krobertson 19:8c1f2a2204fb 107 }while(getTCPConStatus);
krobertson 18:e72ee7aed088 108 }
krobertson 20:81d5655fecc2 109
krobertson 20:81d5655fecc2 110 void clearRXBuffer(){
krobertson 20:81d5655fecc2 111 while(outputDevice.readable()){
krobertson 20:81d5655fecc2 112 outputDevice.getc();
krobertson 20:81d5655fecc2 113 }
krobertson 20:81d5655fecc2 114 }
krobertson 20:81d5655fecc2 115
krobertson 41:df156ae5631b 116 char rx_ready_with_timeout(Serial* serialDevice = NULL, float seconds = 0, unsigned int u_seconds = 0){
krobertson 36:53b69e471b5a 117 if(serialDevice == NULL){
krobertson 36:53b69e471b5a 118 serialDevice = &outputDevice;
krobertson 36:53b69e471b5a 119 }
krobertson 41:df156ae5631b 120 if(seconds==0 && u_seconds==0){
krobertson 41:df156ae5631b 121 seconds = 3.0;
krobertson 41:df156ae5631b 122 }
krobertson 36:53b69e471b5a 123 if(serialDevice->readable()){
krobertson 20:81d5655fecc2 124 return 1;
krobertson 20:81d5655fecc2 125 }else{
krobertson 20:81d5655fecc2 126 EvTimer t;
krobertson 41:df156ae5631b 127 if(seconds>0){
krobertson 41:df156ae5631b 128 t.set_s_period(seconds);
krobertson 41:df156ae5631b 129 }else{
krobertson 41:df156ae5631b 130 t.set_us_period(u_seconds);
krobertson 41:df156ae5631b 131 }
krobertson 20:81d5655fecc2 132 t.start_timer();
krobertson 20:81d5655fecc2 133 while(t.get_num_trips() == 0){
krobertson 36:53b69e471b5a 134 if(serialDevice->readable()){
krobertson 20:81d5655fecc2 135 return 1;
krobertson 20:81d5655fecc2 136 }
krobertson 20:81d5655fecc2 137 }
krobertson 20:81d5655fecc2 138 t.stop_timer();
krobertson 20:81d5655fecc2 139 }
krobertson 20:81d5655fecc2 140 return 0;
krobertson 20:81d5655fecc2 141 }
krobertson 20:81d5655fecc2 142
krobertson 20:81d5655fecc2 143 char getSynced(){
krobertson 20:81d5655fecc2 144 int zero_count = 0; //count of the number of consecutive 0's
krobertson 20:81d5655fecc2 145 char sel_ret;
krobertson 20:81d5655fecc2 146 unsigned char temp;
krobertson 20:81d5655fecc2 147 while(1){
krobertson 20:81d5655fecc2 148 if((sel_ret = rx_ready_with_timeout()) != 1){
krobertson 20:81d5655fecc2 149 return sel_ret;
krobertson 20:81d5655fecc2 150 }
krobertson 20:81d5655fecc2 151 temp = outputDevice.getc();
krobertson 20:81d5655fecc2 152 //USB::getSerial().printf("%x ",temp);
krobertson 20:81d5655fecc2 153 if(temp==0){
krobertson 20:81d5655fecc2 154 zero_count++;
krobertson 20:81d5655fecc2 155 }else if(temp == 0xFF){
krobertson 20:81d5655fecc2 156 printf("TESTING SYNC COMPLETE: %d. expecting %d\n",zero_count,sizeof(PacketStruct)-sizeof(char));
krobertson 20:81d5655fecc2 157 if(zero_count == sizeof(PacketStruct)-sizeof(char)){
krobertson 20:81d5655fecc2 158 USB::getSerial().printf("!!!!DONE SYNCING!!!!\r\n");
krobertson 20:81d5655fecc2 159 return 1;
krobertson 20:81d5655fecc2 160 }
krobertson 20:81d5655fecc2 161 }else{
krobertson 20:81d5655fecc2 162 zero_count = 0;
krobertson 20:81d5655fecc2 163 }
krobertson 20:81d5655fecc2 164 if(zero_count > sizeof(PacketStruct)-sizeof(char)){
krobertson 20:81d5655fecc2 165 zero_count = 0;
krobertson 20:81d5655fecc2 166 }
krobertson 20:81d5655fecc2 167 }
krobertson 20:81d5655fecc2 168 }
krobertson 20:81d5655fecc2 169
krobertson 20:81d5655fecc2 170 char receivePacket(PacketStruct* pack){
krobertson 20:81d5655fecc2 171 char rx_ret;
krobertson 20:81d5655fecc2 172 char temp;
krobertson 20:81d5655fecc2 173 int total_rec_bytes = 0;//total bytes received for this packet.
krobertson 20:81d5655fecc2 174 while(total_rec_bytes < sizeof(PacketStruct)){
krobertson 20:81d5655fecc2 175 if((rx_ret = rx_ready_with_timeout()) != 1){
krobertson 20:81d5655fecc2 176 USB::getSerial().printf("timed out waiting for packet. Received %d/%d bytes\r\n",total_rec_bytes,sizeof(PacketStruct));
krobertson 20:81d5655fecc2 177 return rx_ret;
krobertson 20:81d5655fecc2 178 }
krobertson 20:81d5655fecc2 179 temp = outputDevice.getc();
krobertson 20:81d5655fecc2 180 *(((char*)pack) + total_rec_bytes) = temp;
krobertson 20:81d5655fecc2 181 total_rec_bytes++;
krobertson 20:81d5655fecc2 182 }
krobertson 20:81d5655fecc2 183 return 1;
krobertson 20:81d5655fecc2 184 }
krobertson 20:81d5655fecc2 185
dylanembed123 12:e42985e3ea64 186 unsigned int min(unsigned int a,unsigned int b){
dylanembed123 12:e42985e3ea64 187 return a<b ? a : b;
dylanembed123 12:e42985e3ea64 188 }
krobertson 20:81d5655fecc2 189
dylanembed123 12:e42985e3ea64 190 void sendPacket(unsigned int superPackID,char* data,unsigned int size,PACKET_TYPE type = PT_END){
dylanembed123 12:e42985e3ea64 191 if(data!=NULL && size>0){
dylanembed123 13:a6d3cf2b018e 192 for(int i=0;i<=(size-1)/PACKETSIZE;i++){
dylanembed123 12:e42985e3ea64 193 PacketStruct output;
dylanembed123 12:e42985e3ea64 194 output.type=PT_DEFAULT;
dylanembed123 12:e42985e3ea64 195 output.superPackID=superPackID;
dylanembed123 12:e42985e3ea64 196 output.size=min(PACKETSIZE,size-i*PACKETSIZE);
dylanembed123 14:6be57da62283 197 for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
dylanembed123 14:6be57da62283 198 for(int a=0;a<output.size;a++){output.data[a]=data[a-i*PACKETSIZE];}
dylanembed123 13:a6d3cf2b018e 199 for(int a=output.size;a<PACKETSIZE;a++){output.data[a]='\0';}
dylanembed123 12:e42985e3ea64 200 sendPacket(output);
dylanembed123 12:e42985e3ea64 201 }
dylanembed123 12:e42985e3ea64 202 }else{
dylanembed123 12:e42985e3ea64 203 PacketStruct output;
dylanembed123 12:e42985e3ea64 204 output.type=type;
dylanembed123 13:a6d3cf2b018e 205 output.size=0;
dylanembed123 12:e42985e3ea64 206 output.superPackID=superPackID;
dylanembed123 14:6be57da62283 207 for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
dylanembed123 14:6be57da62283 208 // Check for empty packet
dylanembed123 14:6be57da62283 209 if(output.type==PT_EMPTY){output.type=0;output.size=0;output.superPackID=0;output.special[3]=0xFF;}
dylanembed123 14:6be57da62283 210 for(int a=0;a<PACKETSIZE;a++){output.data[a]='\0';}
dylanembed123 12:e42985e3ea64 211 sendPacket(output);
dylanembed123 12:e42985e3ea64 212 }
dylanembed123 12:e42985e3ea64 213 }
krobertson 20:81d5655fecc2 214
dylanembed123 16:4f5d20b87dc3 215 PacketStruct* lastValid;
dylanembed123 16:4f5d20b87dc3 216 bool found;
dylanembed123 16:4f5d20b87dc3 217 void handleUpdate(){
dylanembed123 16:4f5d20b87dc3 218 USB::getSerial().printf("Interupt\n");
dylanembed123 16:4f5d20b87dc3 219 PacketStruct* next = getNextPacket();
dylanembed123 16:4f5d20b87dc3 220 if(next!=NULL){lastValid=next;}
dylanembed123 16:4f5d20b87dc3 221 }
dylanembed123 14:6be57da62283 222
dylanembed123 14:6be57da62283 223 // Number of consecutive zeros
dylanembed123 14:6be57da62283 224 unsigned int numZeros;
dylanembed123 14:6be57da62283 225 // Return true if a resync command has been received
dylanembed123 14:6be57da62283 226 bool resetCheck(char input){
dylanembed123 14:6be57da62283 227 if(input=='\0'){
dylanembed123 14:6be57da62283 228 numZeros++;
dylanembed123 14:6be57da62283 229 }else if(numZeros==sizeof(PacketStruct)-1&&input==0xFF){
dylanembed123 14:6be57da62283 230 return true;
dylanembed123 14:6be57da62283 231 }else{
dylanembed123 14:6be57da62283 232 numZeros=0;
dylanembed123 14:6be57da62283 233 }
dylanembed123 14:6be57da62283 234 return false;
dylanembed123 14:6be57da62283 235 }
dylanembed123 14:6be57da62283 236
dylanembed123 14:6be57da62283 237 // Temperary storage for next valid
dylanembed123 14:6be57da62283 238 PacketStruct* next;
dylanembed123 14:6be57da62283 239 // Number of valid bits in next packet
dylanembed123 14:6be57da62283 240 int nextValid;
dylanembed123 14:6be57da62283 241 /// \brief Grab the next packet
dylanembed123 12:e42985e3ea64 242 PacketStruct* getNextPacket(){
dylanembed123 14:6be57da62283 243 // Check for null packet
dylanembed123 14:6be57da62283 244 if(next==NULL){next=new PacketStruct();nextValid=0;}
dylanembed123 14:6be57da62283 245 // Create reset packet which resets on re-sync command
dylanembed123 14:6be57da62283 246 bool resetPacket=false;
dylanembed123 14:6be57da62283 247
dylanembed123 14:6be57da62283 248 // While there is data to read
dylanembed123 14:6be57da62283 249 while(0<outputDevice.readable()){
dylanembed123 14:6be57da62283 250 // Check if a full packet has been received
dylanembed123 14:6be57da62283 251 if(nextValid==sizeof(PacketStruct))break;
dylanembed123 14:6be57da62283 252 // Read in next char
dylanembed123 14:6be57da62283 253 char input=outputDevice.getc();
dylanembed123 16:4f5d20b87dc3 254 USB::getSerial().printf("Read ByteC %X %X\n",nextValid,input);
dylanembed123 14:6be57da62283 255 // Check for valid char
dylanembed123 14:6be57da62283 256 if(resetCheck(input)){resetPacket=true;break;}
dylanembed123 14:6be57da62283 257 // Set char
dylanembed123 14:6be57da62283 258 ((char*)next)[nextValid++] = input;
dylanembed123 14:6be57da62283 259 }
dylanembed123 14:6be57da62283 260
dylanembed123 14:6be57da62283 261 if(nextValid==sizeof(PacketStruct)||resetPacket){
dylanembed123 14:6be57da62283 262 // Reset packet
dylanembed123 14:6be57da62283 263 PacketStruct* output=next;next=NULL;
dylanembed123 14:6be57da62283 264 // Return
dylanembed123 14:6be57da62283 265 return resetPacket?NULL:output;
dylanembed123 14:6be57da62283 266 }
dylanembed123 14:6be57da62283 267 return NULL;
dylanembed123 14:6be57da62283 268 /*
dylanembed123 12:e42985e3ea64 269 int avail = outputDevice.readable();
dylanembed123 12:e42985e3ea64 270 if(avail <= 0)return NULL;
dylanembed123 12:e42985e3ea64 271 PacketStruct* output=new PacketStruct();
dylanembed123 12:e42985e3ea64 272 for(int i=0;i<sizeof(PacketStruct);i++){
dylanembed123 12:e42985e3ea64 273 // Wait for byte
dylanembed123 12:e42985e3ea64 274 while(outputDevice.readable()<=0){}
dylanembed123 12:e42985e3ea64 275 ((char*)output)[i] = outputDevice.getc();
dylanembed123 12:e42985e3ea64 276 }
dylanembed123 12:e42985e3ea64 277 return output;
dylanembed123 14:6be57da62283 278 */
dylanembed123 12:e42985e3ea64 279 }
dylanembed123 12:e42985e3ea64 280
dylanembed123 12:e42985e3ea64 281 };
dylanembed123 12:e42985e3ea64 282 static PacketSender* ps=NULL;
dylanembed123 12:e42985e3ea64 283 static PacketSender& getPS(){
dylanembed123 12:e42985e3ea64 284 if(ps==NULL)ps=new PacketSender();
dylanembed123 12:e42985e3ea64 285 return *ps;
dylanembed123 12:e42985e3ea64 286 }
krobertson 20:81d5655fecc2 287
krobertson 20:81d5655fecc2 288 #endif