Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / lib / colonet_wireless / colonet_wireless.cpp @ 11

History | View | Annotate | Download (16.1 KB)

1
/** @file colonet_wireless.cpp 
2
 *
3
 * @brief Implementation of server-side colonet wireless library
4
 *
5
 * @author Eugene Marinelli
6
 *
7
 * @bug Only reads when gtkterm is open
8
 * @bug Fails to tokenize
9
 * @bug Deconstructor not implemented
10
 * @bug Nonsense initializations in constructor
11
 */
12

    
13
/********************************* Includes **********************************/
14
#include <string>
15
#include <pthread.h>
16
#include <iostream>
17
#include <fstream>
18
#include <unistd.h>
19
#include <fcntl.h>
20
#include "colonet_wireless.h"
21

    
22
/******************************* Definitions *********************************/
23
#define TIMEOUT_MAX 100
24
#define WL_SEND_DELAY 5 //microseconds
25

    
26
//for debug printouts
27
#define DEBUG 1
28

    
29
#ifdef DEBUG
30
#define dbg_printf(...) printf(__VA_ARGS__)
31
#define dbg_fprintf(...) fprintf(__VA_ARGS__)
32
#else
33
#define dbg_printf(...)
34
#define dbg_fprintf(...)
35
#endif
36

    
37
typedef struct {
38
  MsgHandlerFunction handler;
39
  ColonetWireless* cw;
40
} ListenerArgs;
41

    
42
/************************* Internal prototypes *******************************/
43
static void* listen(void* args);
44

    
45
/**************************** Public functions *******************************/
46
ColonetWireless::ColonetWireless(char* wl_port, 
47
                                 MsgHandlerFunction message_handler_, 
48
                                 char* log_filename_, bool join_token_ring_,
49
                                 bool ignore_token_)
50
{
51
  have_token = false;
52
  first_packet_sent = false;
53

    
54
  //FIX THESE INITIALIZATIONS
55
  token_src = 255;
56
  token_dest = 255;
57
  num_robots = 0;
58
  connected = false;
59
  ignore_token = ignore_token_;
60

    
61
  sprintf(wireless_port, wl_port);
62

    
63
  if (log_filename) {
64
    logging_enabled = true;
65
    sprintf(log_filename, log_filename_);
66
  }
67

    
68
  join_token_ring = join_token_ring_;
69

    
70
  message_handler = message_handler_;
71
}
72

    
73
ColonetWireless::~ColonetWireless()
74
{
75
  //kill listener thread
76
  
77

    
78
  //free packets in queues
79
  
80
}
81

    
82
int ColonetWireless::handle_packet(ColonetPacket* pkt)
83
{
84
  if (logging_enabled) {
85
    /* Log the packet */
86
    log_packet(pkt);
87
  }
88

    
89
  /* Update what we know about the network based on the packet */
90
  update_network_props(pkt);
91

    
92
  /* Received a message */
93
  if (message_handler(pkt) < 0) {
94
    fprintf(stderr, "%s: message_handler failed\n", __FUNCTION__);
95
    return -1;
96
  }
97
  
98
  if ((unsigned char) pkt->token_dest == WL_ERROR_DEST) {
99
    /* token_dest is an error - handle it */
100
    if (handle_error(pkt) < 0) {
101
      fprintf(stderr, "%s: handle_error failed\n", __FUNCTION__);
102
      return -1;
103
    }
104
  } else if(join_token_ring && !connected) {
105
    /* We want to join the token ring but we are not connected - join robot 
106
     * network */
107
    dbg_printf("%s: Trying to join robot network...\n", __FUNCTION__);
108

    
109
    if (join_network(pkt) < 0) {
110
      dbg_fprintf(stderr, "%s: join_network failed\n", __FUNCTION__);
111
      return -1;
112
    }
113
  } else {
114

    
115
    if (pkt->token_dest == token_src && join_token_ring && !ignore_token) {
116
      /* Received the token - send queued message(s) */
117
      dbg_printf("%s: token received\n", __FUNCTION__);
118

    
119
      have_token = true;
120
    
121
      if (transmit_queued_message() < 0) {
122
        fprintf(stderr, "%s: transmit_queued_message failed\n", __FUNCTION__);
123
        return -1;
124
      } else {
125
        if (!first_packet_sent) {
126
          dbg_printf("First packet sent.\n");
127
          first_packet_sent = true;
128
        }
129
      }
130
    }
131
  }
132

    
133
  return 0;
134
}
135

    
136
pthread_t* ColonetWireless::run_listener_thread()
137
{
138
  // pthread_t listener_thread;
139

    
140
  pthread_t* listener_thread = (pthread_t*)malloc(sizeof(pthread_t));
141

    
142
  dbg_printf("Spawning listener thread...\n");
143

    
144
  ListenerArgs* args = new ListenerArgs;
145
  args->cw = this;
146

    
147
  if (pthread_create(listener_thread, NULL, listen, args) != 0) {
148
    perror("pthread_create");
149
    return NULL;
150
  }
151

    
152
  return listener_thread;
153
}
154

    
155
int ColonetWireless::send(unsigned char msg_dest, unsigned char msg_type, 
156
                          unsigned char msg_code, unsigned char* data)
157
{
158
  /* Create a new packet it and queue it to be sent */
159
  ColonetPacket* newpkt = create_packet(token_src, token_dest, msg_dest, 
160
                                        msg_type, msg_code, data, num_robots);
161

    
162
  if(newpkt == NULL){
163
    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);
164
    return -1;
165
  }
166

    
167
  if (ignore_token) {
168
    send_packet(newpkt);
169
  } else {
170
    outbox.push(newpkt);                
171
  }
172

    
173
  return 0;
174
}
175

    
176
void ColonetWireless::clear_message_queue(void)
177
{
178
  while (!outbox.empty()) {
179
    outbox.pop();
180
  }
181
}
182

    
183
/**************************** Private functions ******************************/
184

    
185
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
186
 * Listens for bytes on the wireless port and constructs packets based on them.
187
 * Not part of the ColonetWireless class since a function pointer must be
188
 * passed in starting the thread that runs it (tricky or impossible if it's 
189
 * a class function).
190
 *
191
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
192
 * @return NULL
193
 */
194
static void* listen(void* args)
195
{
196
  int read_len;
197
  int portfd;
198
  unsigned char inbuf[128];
199
  int no_packet_timeout_count = 0;
200
  ColonetPacket* in_pkt;
201

    
202
  /* Extract arguments from args */
203

    
204
  ColonetWireless* cw = ((ListenerArgs*)args)->cw;
205

    
206
  /* Open connection to wireless port */
207
  if((portfd = open(cw->wireless_port, O_RDWR)) < 0){
208
    perror("open");
209
    pthread_exit(NULL);
210
  }
211

    
212
  /* loop waiting for messages */
213
  while(1){
214
    /* Read in some bytes (blocks until bytes are read) */
215
    read_len = read(portfd, inbuf, 3);
216

    
217
    if(read_len > 0){
218
      /* Read in some bytes - this will always be true if portfd is blocking */
219
      /* parse the packet or part of packet */
220
      in_pkt = cw->build_packet(inbuf, read_len);
221
      
222
      if(in_pkt == NULL){
223
        /* Packet is not ready yet - go back and read some more bytes */
224
        continue;
225
      }else{
226
        /* Packet is complete */
227
        /* close fd so that we can safely write to it in the handler */
228
        close(portfd);
229

    
230
        if (cw->handle_packet(in_pkt)) {
231
          fprintf(stderr, "%s: Error - handle_packet failed\n", __FUNCTION__);
232
        }
233

    
234
        /* reopen wireless port */
235
        if(!(portfd = open(cw->wireless_port, O_RDWR))){
236
          perror("open");
237
          pthread_exit(NULL);
238
        }
239
      }
240
    }else if(read_len == 0){
241
      // Note - this won't actually happen if read blocks
242
      if(no_packet_timeout_count++ > TIMEOUT_MAX){
243
        //Nothing received for a while - try to start a new token ring
244
        dbg_printf("Attempting to start a token ring...\n");
245

    
246
        cw->token_src = 0;
247
        cw->token_dest = 1;
248
        cw->num_robots = 0;
249
  
250
        ColonetPacket* tmp_pkt = cw->create_packet(cw->token_src, 
251
                                                   cw->token_dest, 0, 0, 0, 
252
                                                   (unsigned char*) "",
253
                                                   cw->num_robots);
254
        cw->send_packet(tmp_pkt);
255
        free(tmp_pkt);
256
      }
257
    }else{ //(read_len < 0)
258
      //Error
259
      perror("read");
260
    }
261
  }
262

    
263
  pthread_exit(NULL);
264

    
265
  return NULL;
266
}
267

    
268
/** @brief Takes some number of bytes and tries to build a packet with them
269
 * State of the function is saved between callings, so parts of packets 
270
 * can be provided as available
271
 *
272
 * @param buf Data buffer 
273
 * @param len Number of bytes to build with
274
 */
275
ColonetPacket* ColonetWireless::build_packet(unsigned char* buf, int len)
276
{
277
  static unsigned char packet_buf[PACKET_SIZE];
278
  static int buf_pos = 0;
279

    
280
  /* If this is pointer is returned - will either point to NULL or a complete
281
   * packet */
282
  ColonetPacket* complete_packet = NULL;
283

    
284
  if (buf_pos >= PACKET_SIZE) {
285
    buf_pos = 0;
286
  }
287

    
288
  for (int i = 0; i < len; i++) {
289
    dbg_printf("%d ", buf[i]);
290

    
291
    switch(buf_pos){
292
    case 0: //prefix0
293
      if(buf[i] == 'R'){
294
        memset(packet_buf, 0, PACKET_SIZE);
295
        packet_buf[0] = 'R';
296
        buf_pos = 1;
297
      }
298
      break;
299
    case 1: //prefix1
300
      if(buf[i] == 'C'){
301
        packet_buf[1] = 'C';
302
        buf_pos = 2;
303
      }else{
304
        buf_pos = 0;
305
      }
306
      break;
307
    default: 
308
      /* anything except prefix0, prefix1, or checksum */
309
      
310
      /* Check for R,C - in this case, very likely that we missed a byte */
311
      if (i > 0 && buf[i] == 'C' && buf[i-1] == 'R') {
312
        packet_buf[0] = 'R';
313
        packet_buf[1] = 'C';
314
        
315
        buf_pos = 2;
316
      } else {
317
        packet_buf[buf_pos] = buf[i];
318
        buf_pos++;
319
      }
320
      break;
321
    case (PACKET_SIZE-1):
322
      /* At checksum location */
323
      buf_pos = 0;
324
      packet_buf[15] = buf[i];
325

    
326
      /* Compute desired checksum */
327
      unsigned char sum = 0;
328
      for(int j = 2; j < PACKET_SIZE; j++){
329
        sum += packet_buf[j];
330
      }
331
      
332
      if(!sum){
333
        //Checksum passed
334
        dbg_printf("%s: checksum passed\n", __FUNCTION__);
335

    
336
        complete_packet = create_packet(packet_buf[2], packet_buf[3], 
337
                                        packet_buf[4], packet_buf[5], 
338
                                        packet_buf[6], (packet_buf+7), 
339
                                        packet_buf[PACKET_SIZE-2]);
340
      }else{
341
        dbg_printf("%s: checksum failed\n", __FUNCTION__);
342
      }
343

    
344
      break;
345
    }
346
  }
347

    
348
  return complete_packet;
349
}
350

    
351
void ColonetWireless::update_network_props(ColonetPacket* pkt)
352
{
353
  if(first_packet_sent && pkt->token_dest == token_dest){
354
    /* Another robot is sending to our dest - therefore the network
355
     * doesn't know about us yet.  Try to register again. */
356
    dbg_printf("Disconnected from network\n");
357
    dbg_printf("%s: failed to register in network; trying again...\n", 
358
               __FUNCTION__);
359
    connected = false;
360

    
361
  }
362

    
363
  if(num_robots < pkt->num_robots){    
364
    if(token_dest == 0){
365
      /* If we were the last robot, we now point to newly added robot */
366
      token_dest = num_robots;
367
      dbg_printf("%s: token_dest updated to %d\n", __FUNCTION__, token_dest);
368
    }
369

    
370
    num_robots = pkt->num_robots;
371

    
372
    dbg_printf("%s: num_robots updated to %d\n", __FUNCTION__, num_robots);
373
  }else if(num_robots > pkt->num_robots && first_packet_sent){
374
    /* A robot has left the network... hopefully not us */
375
    dbg_printf("%s: number of robots decreased to %d\n", __FUNCTION__, 
376
               pkt->num_robots);
377

    
378
    num_robots = pkt->num_robots;
379
  }
380
}
381

    
382
int ColonetWireless::join_network(ColonetPacket* pkt)
383
{
384
  dbg_printf("Attempting to join network...\n");
385

    
386
  token_src = pkt->num_robots;
387
  token_dest = 0;
388
  num_robots = pkt->num_robots+1;
389

    
390
  ColonetPacket* newpkt = create_packet(token_src, token_dest, 0, 0, 0, 
391
                                        (unsigned char*)"", num_robots);
392
  if(newpkt == NULL){
393
    fprintf(stderr, "Error: %s - create_packet failed\n", __FUNCTION__);
394
    return -1;
395
  }
396

    
397
  dbg_printf("JOIN PACKET:\n"
398
             "token_src:%d\n"
399
             "token_dest:%d\n"
400
             "msg_dest:%d\n"
401
             "msg_type:%d\n"
402
             "msg_code:%d\n"
403
             "num_robots:%d\n",
404
             newpkt->token_src, newpkt->token_dest, newpkt->msg_dest, 
405
             newpkt->msg_type, newpkt->msg_code, newpkt->num_robots);
406
  
407
  dbg_printf("data: ");
408

    
409
  for(int i = 0; i < PACKET_DATA_LEN; i++){
410
    dbg_printf("%d ", newpkt->data[i]);
411
  }
412
  dbg_printf("\n");
413

    
414
  send_packet(newpkt);
415

    
416
  connected = true;
417
  dbg_printf("Connected to network.\n");
418

    
419
  free(newpkt);
420

    
421
  return 0;
422
}
423

    
424
int ColonetWireless::transmit_queued_message()
425
{
426
  ColonetPacket* top_pkt;
427
  int ret;
428

    
429
  if(have_token){
430
    top_pkt = outbox.front();
431
    if(top_pkt == NULL){
432
      dbg_printf("%s: no messages to send -- Passing the token.\n", 
433
                 __FUNCTION__);
434
      /* If no packets need to be sent, just send a blank packet */
435
      top_pkt = create_packet(token_src, token_dest, 0, 0, 0, 
436
                              (unsigned char*)"", num_robots);
437
    }else{
438
      outbox.pop();
439
    }
440

    
441
    ret = send_packet(top_pkt);
442
    free(top_pkt);
443
    if(ret < 0){
444
      dbg_fprintf(stderr, "%s: send_packet failed\n", __FUNCTION__);
445
      return -1;
446
    }
447

    
448
    dbg_printf("%s: packet sent\n", __FUNCTION__);
449

    
450
    have_token = false;
451
  }
452

    
453
  return 0;
454
}
455

    
456
int ColonetWireless::send_packet(ColonetPacket* pkt)
457
{
458
  int portfd = open(wireless_port, O_RDWR);
459

    
460
  dbg_printf("\nsend packet: \n");
461

    
462
  if (portfd == 0) {
463
    return -1;
464
  }
465

    
466
  //write(portfd, (unsigned char*)pkt, 16);
467

    
468
  for (int i = 0; i < PACKET_SIZE; i++) {
469
    write(portfd, (unsigned char*)pkt+i, 1);
470
    usleep(50); //delay here - robot wireless can't handle full data rate...
471
    dbg_fprintf(stderr, "%x ", ((unsigned char*)pkt)[i]);
472
  }
473
  dbg_printf("\n");
474

    
475
  close(portfd);
476

    
477
  return 0;
478
}
479

    
480
unsigned char ColonetWireless::compute_checksum(ColonetPacket* pkt)
481
{
482
  char checksum = 0;
483

    
484
  checksum -= pkt->token_src;
485
  checksum -= pkt->token_dest;
486
  checksum -= pkt->msg_dest;
487
  checksum -= pkt->msg_type;
488
  checksum -= pkt->msg_code;
489

    
490
  for(int i = 0; i < PACKET_DATA_LEN; i++){
491
    checksum -= pkt->data[i];
492
  }
493

    
494
  checksum -= pkt->num_robots;
495

    
496
  return checksum;
497
}
498

    
499
ColonetPacket* ColonetWireless::create_packet(unsigned char token_src_, 
500
                                              unsigned char token_dest_,
501
                                              unsigned char msg_dest, 
502
                                              unsigned char msg_type, 
503
                                              unsigned char msg_code, 
504
                                              unsigned char* data,
505
                                              unsigned char num_robots_)
506
{
507
  ColonetPacket* dest_packet = new ColonetPacket;
508

    
509
  dest_packet->prefix[0] = 'R';
510
  dest_packet->prefix[1] = 'C';
511

    
512
  dest_packet->token_src = token_src_;
513
  dest_packet->token_dest = token_dest_;
514
    
515
  dest_packet->msg_dest = msg_dest;
516
  dest_packet->msg_type = msg_type;
517
  dest_packet->msg_code = msg_code;
518

    
519
  memset(dest_packet->data, 0, PACKET_DATA_LEN);
520
  memcpy(dest_packet->data, (char*)data, PACKET_DATA_LEN);
521

    
522
  dest_packet->num_robots = num_robots_;
523
  dest_packet->checksum = compute_checksum(dest_packet);
524

    
525
  //printf("Made this packet: \n");
526
  //print_packet(dest_packet);
527

    
528
  return dest_packet;
529
}
530

    
531
void ColonetWireless::print_packet(ColonetPacket* pkt)
532
{
533
  int i;
534

    
535
  printf("Prefix: %c%c\n", pkt->prefix[0], pkt->prefix[1]);
536
  printf("Token src: %d\n", pkt->token_src);
537
  printf("Token dest: %d\n", pkt->token_dest);
538
  printf("Msg dest: %d\n", pkt->msg_dest);
539
  printf("Msg type: %d\n", pkt->msg_type);
540
  printf("Msg code: %d\n", pkt->msg_code);
541

    
542
  printf("Data: ");
543
  for(i = 0; i < PACKET_DATA_LEN; i++){
544
    printf("%d ", pkt->data[i]);
545
  }
546
  printf("\n");
547
  
548
  printf("Num robots: %d\n", pkt->num_robots);
549
  printf("Checksum: %d\n", pkt->checksum);
550
}
551

    
552
int ColonetWireless::handle_error(ColonetPacket* pkt)
553
{
554
  int dead_robot;
555
  int errcode = ((char*)pkt)[WL_ERROR_LOC];
556

    
557
  dbg_printf("%s: Handling error %d\n", __FUNCTION__, pkt->token_dest);
558

    
559
  switch(errcode){ //token_dest 
560
  case WL_ERRCODE_ROBOTDEATH:
561
    dead_robot = ((char*)pkt)[WL_ERR_DEAD_LOC];
562

    
563
    fprintf(stderr, "Dead robot: %d\n", dead_robot);
564
    num_robots--;
565
    
566
    if(token_src > dead_robot){
567
      if(token_src == num_robots){
568
        token_src--;
569
        token_dest = 0;
570
      }else{
571
        token_src--;
572
        token_dest--;
573
      }
574
    }
575

    
576
    //ensure that the last robot talks to the 0th one
577
    //this won't be set if the LAST robot dies (all robots are < dead)
578
    if(token_src == num_robots - 1){
579
      token_dest = 0;
580
    }
581

    
582
    if(token_src == dead_robot){
583
      //you took the place of the dead robot
584
      //you start the token ring again
585
      ColonetPacket* tmp_pkt = create_packet(token_src, token_dest, 0, 0, 0,
586
                                             (unsigned char*)"", num_robots);
587
      send_packet(tmp_pkt);
588
      free(tmp_pkt);
589
    }
590
    break;
591
  case WL_ERRCODE_NEEDTOCHARGE:
592
    fprintf(stderr, "Need to charge error received\n");
593
    break;
594
  case WL_ERRCODE_CHARGESTATION:
595
    fprintf(stderr, "Charge station error received\n");
596
    break;
597
  }
598
    
599
  return 0;
600
}
601

    
602
/** @brief 
603
 *
604
 * @param pkt Packet to be logged
605
 *
606
 * @return 0 on success, -1 on failure
607
 */
608
int ColonetWireless::log_packet(ColonetPacket* pkt)
609
{
610
  FILE* logfile = fopen(log_filename, "a");
611

    
612
  if (logfile == NULL) {
613
    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
614
    return -1;
615
  }
616

    
617
  for (int i = 0; i < PACKET_SIZE; i++) {
618
    fprintf(logfile, "%d ", *((unsigned char*)pkt+i));
619
  }
620

    
621
  fprintf(logfile, "\n");
622

    
623
  fclose(logfile);
624

    
625
  return 0;
626
}