Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

packet.h@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
51:d6b64ac3c30d 
Final Project;

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