Statistics
| Revision:

root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 681

History | View | Annotate | Download (10.8 KB)

1
/** @file colonet_wireless.c
2
 *
3
 * @brief Implementation of server-side colonet wireless library
4
 *
5
 * @author Eugene Marinelli
6
 */
7

    
8
/********************************* Includes **********************************/
9
#include <string>
10
#include <pthread.h>
11
#include <iostream>
12
#include <fstream>
13
#include <unistd.h>
14
#include <fcntl.h>
15
#include <signal.h>
16

    
17
#include <ColonetServer.h>
18
#include <wireless.h> // Colonet wireless library.
19
#include <wl_token_ring.h>
20

    
21
#include <colonet_defs.h>
22
#include <colonet_wireless.h>
23

    
24
/******************************* Definitions *********************************/
25

    
26
//Enable 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
extern ColonetServer colonet_server;
38

    
39
static bool logging_enabled;
40
static char log_filename[80];
41
static pthread_t listener_thread;
42
static char wl_port[40];
43

    
44
/************************* Internal prototypes *******************************/
45
static void* listen(void* args);
46
static void timeout_handler(void);
47
static void handle_response(int frame, int received);
48
static void handle_receive(char type, int source, unsigned char* packet, int len);
49
static void unregister(void);
50
static int log_packet(unsigned char* packet, int len);
51

    
52
/**************************** Public functions *******************************/
53
/**
54
 * @brief Initializes the wireless library
55
 *
56
 * @param wl_port_ The port to listen for wireless messages on
57
 * @param log_filename_ The name of the log file
58
 *
59
 * @return 0 on success, negative error code on failure
60
 */
61
int colonet_wl_init(char* wl_port_, char* log_filename_) {
62
  if (log_filename_ != NULL) {
63
    logging_enabled = true;
64
    strcpy(log_filename, log_filename_);
65
  }
66

    
67
  strncpy(wl_port, wl_port_, 40);
68

    
69
  wl_set_com_port(wl_port);
70

    
71
  fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);
72
  if (wl_init() != 0) {
73
    fprintf(stderr, "wl_init failed.\n");
74
    return -1;
75
  }
76

    
77
  wl_token_ring_register();
78

    
79
  fprintf(stderr, "Joining token ring...\n");
80
  if (wl_token_ring_join() != 0) {
81
    fprintf(stderr, "Failed to join token ring.\n");
82
    return -1;
83
  }
84
  fprintf(stderr, "Joined token ring.\n");
85

    
86
  return 0;
87
}
88

    
89
/**
90
 * @brief This function will kill the thread that is listening for wireless messages
91
 */
92
void colonet_wl_kill_listener_thread() {
93
  pthread_kill(listener_thread, 9);
94
}
95

    
96
/**
97
 * @brief This function spawns a thread to listen for wireless messages
98
 *
99
 * @return 0 on success, negative error code on failure
100
 */
101
int colonet_wl_run_listener_thread() {
102
  dbg_printf("Spawning listener thread...\n");
103

    
104
  if (pthread_create(&listener_thread, NULL, listen, NULL)) {
105
    perror("pthread_create");
106
    return -1;
107
  }
108

    
109
  return 0;
110
}
111

    
112
/**
113
 * @brief This function sends a message out on the wireless
114
 *
115
 * @param client_source The index in the connection pool of the client that is sending the message
116
 * @param dest The robot that is meant to receive this message
117
 * @param msg_type The type of the message
118
 * @param msg_code The message code
119
 * @param args The arguments to send with the message
120
 *
121
 * @return 0 on success, a negative error code on failure
122
 */
123
int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code,
124
                    unsigned char* args) {
125
  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
126

    
127
  //create and set up new packet to be sent.
128
  ColonetRobotServerPacket pkt;
129
  pkt.client_id = client_source;
130
  pkt.msg_code = msg_code;
131

    
132
  //copy over the args into the data portion of the packet
133
  if (args != NULL) {
134
    for (int i = 0; i < PACKET_DATA_LEN; i++) {
135
      printf("data %i: %u\n", i, args[i]);
136
      pkt.data[i] = args[i];
137
    }
138
  }
139

    
140
  if (dest == GLOBAL_DEST) {
141
    //printf("sending to global dest\n");
142
    int ret = wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
143
                                    sizeof(ColonetRobotServerPacket), 0);
144
    if (ret != 0) {
145
      fprintf(stderr, "%s: wl_send_global_packet failed.\n", __FUNCTION__);
146
      return -1;
147
    }
148
  } else {
149
    //("sending to specific robot: %d.\n", dest);
150
    int ret = wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
151
                                                   sizeof(ColonetRobotServerPacket), dest,
152
                                                   COLONET_RESPONSE_PACKET_FRAME_ID);
153
    if (ret != 0) {
154
      fprintf(stderr, "%s: wl_send_robot_to_robot_global_packet failed.\n", __FUNCTION__);
155
      return -1;
156
    }
157
  }
158

    
159
  return 0;
160
}
161

    
162
/**
163
 * @brief Used to get the number of robots in the token ring
164
 *
165
 * @return The result of wl_token_get_num_robots()
166
 */
167
int colonet_get_num_robots(void) {
168
  return wl_token_get_num_robots();
169
}
170

    
171
/**
172
 * @brief Gets a list of the xbee ids of the ones in the token ring
173
 *
174
 * @param numrobots A pointer to the variable where you want the number of robots to be stored
175
 *
176
 * @return A dynamically allocated piece of memory that contains the list of xbee ids.
177
 *   The function calling this function must free this memory when done with it.
178
 */
179
int* colonet_get_xbee_ids(int* numrobots) {
180
  int num_robots = wl_token_get_num_robots();
181
  int* ids = (int*)malloc(num_robots * sizeof(int));
182

    
183
  wl_token_iterator_begin();
184

    
185
  int i = 0;
186
  while (wl_token_iterator_has_next()) {
187
    ids[i] = wl_token_iterator_next();
188
    i++;
189
  }
190

    
191
  *numrobots = num_robots;
192
  return ids;
193
}
194

    
195
/**
196
 * @brief Gets the sensor matrix from the wireless library
197
 *
198
 * @param numrobots A pointer to the variable where you want the number of robots to be stored
199
 * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids.  This memory is dynamically
200
 * allocated and should be freed by the calling process when it is done with it
201
 *
202
 * @return The 2d matrix reprsenting the sensor matrix.  This memory is dynamically allocated and should be freed by
203
 * the calling process when it is done with it
204
 */
205
int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {
206
  int num_robots;
207
  int* ids = colonet_get_xbee_ids(&num_robots);
208

    
209
  int** m = (int**)malloc(num_robots * sizeof(int*));
210
  for (int i = 0; i < num_robots; i++) {
211
    m[i] = (int*)malloc(num_robots * sizeof(int*));
212
  }
213

    
214
  for (int i = 0; i < num_robots; i++) {
215
    for (int j = 0; j < num_robots; j++) {
216
      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);
217
    }
218
  }
219

    
220
  *numrobots = num_robots;
221
  *ids_ = ids;
222
  return m;
223
}
224

    
225
/**************************** Private functions ******************************/
226

    
227
//TODO: fill in the doxygen comments
228
/**
229
 * @brief 
230
 *
231
 * @return Void
232
 */
233
static void timeout_handler() {
234
  // printf("colonet wireless - timeout!\n");
235
}
236

    
237
//TODO: fill in the doxygen comments
238
/**
239
 * @brief 
240
 *
241
 * @param frame
242
 * @param received
243
 *
244
 * @return Void
245
 */
246
static void handle_response(int frame, int received) {
247
  //TODO: These are just here to get rid of compiler warnings
248
  frame = frame;
249
  received = received;
250
}
251

    
252
/**
253
 * @brief When a packet is received via wireless, the data is passed to this function
254
 *
255
 * @param type 
256
 * @param source The robot the packet came from
257
 * @param data The data the robot sent
258
 * @param len The length of the data
259
 *
260
 * @return Void
261
 */
262
static void handle_receive(char type, int source_robot, unsigned char* data, int len) {
263
  type = type; //TODO: This is just here to get rid of compiler warnings.
264

    
265
  //printf("***Got packet from robot***\n");
266

    
267
  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data;
268

    
269
  if (logging_enabled) {
270
    log_packet(data, len);
271
  }
272

    
273
  //see if a robot wants the server to tell it where it is
274
  if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
275
    /* Robot has requested its position. */
276
    int robot_x, robot_y;
277
    PositionMonitor* pm = colonet_server.getPositionMonitor();
278
    int num_robots = pm->getNumVisibleRobots();
279

    
280
    //ask the position monitor for where that robot is.
281
    int ret = pm->getRobotPosition(source_robot, &robot_x, &robot_y);
282
    if (ret != 0 && num_robots != 1) {
283
      fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source_robot);
284

    
285
      if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_ROBOT_LOST, NULL) != 0) {
286
        fprintf(stderr, "colonet_wl_send failed!\n");
287
        exit(1);
288
      }
289
    } else {
290
      if (ret != 0 && num_robots == 1) {
291
        VisionPosition pos = pm->getFirstPosition();
292
        robot_x = pos.x;
293
        robot_y = pos.y;
294
      }
295

    
296
      unsigned char response[80];
297
      response[0] = (robot_x >> 8) & 0xFF;
298
      response[1] = robot_x & 0xFF;
299
      response[2] = (robot_y >> 8) & 0xFF;
300
      response[3] = robot_y & 0xFF;
301

    
302
      fprintf(stderr, "Robot %d requested position.  Its position is %d,%d.\n", source_robot, robot_x, robot_y);
303

    
304
      if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) {
305
        fprintf(stderr, "colonet_wl_send failed!\n");
306
        exit(1);
307
      }
308
    }
309
  } else {
310
    //printf("Calling colonet_server.process_received_wireless_message\n");
311
    char processed_data[80];
312
    sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source_robot, pkt->data);
313

    
314
    //fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);
315

    
316
    colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data));
317
  }
318
}
319

    
320
//TODO: fill in the doxygen comments
321
/**
322
 * @brief 
323
 *
324
 * @return Void
325
 */
326
static void unregister(void) {
327
  printf("unregister\n");
328
}
329

    
330
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
331
 * Listens for bytes on the wireless port and constructs packets based on them.
332
 * Not part of the ColonetWireless class since a function pointer must be
333
 * passed in starting the thread that runs it (tricky or impossible if it's
334
 * a class function).
335
 *
336
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
337
 * @return NULL
338
 */
339
static void* listen(void* args) {
340
  args = args; //TODO: These are just here to get rid of compiler warnings.
341

    
342
  fprintf(stderr, "Called listen.\n");
343

    
344
  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister};
345
  wl_register_packet_group(&pgh);
346

    
347
  while (1) {
348
    wl_do();
349

    
350
    //this sleep is here so the thread this runs in doesn't hog the cpu
351
    usleep(1000);
352
  }
353

    
354
  fprintf(stderr, "Listen is returning.\n");
355

    
356
  wl_terminate();
357
  return NULL;
358
}
359

    
360
/** 
361
 * @brief
362
 *
363
 * @param pkt Packet to be logged
364
 *
365
 * @return 0 on success, -1 on failure
366
 */
367
int log_packet(unsigned char* packet, int len) {
368
  FILE* logfile = fopen(log_filename, "a");
369

    
370
  if (logfile == NULL) {
371
    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
372
    return -1;
373
  }
374

    
375
  for (int i = 0; i < len; i++) {
376
    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
377
  }
378
  fprintf(logfile, "\n");
379

    
380
  fclose(logfile);
381
  return 0;
382
}