root / trunk / code / projects / colonet / lib / colonet_wireless / colonet_wireless.cpp @ 11
History | View | Annotate | Download (16.1 KB)
1 | 11 | emarinel | /** @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 | } |