root / branches / analog / code / projects / colonet / server / colonet_wireless.cpp @ 1390
History | View | Annotate | Download (10.9 KB)
1 | 114 | emarinel | /** @file colonet_wireless.c
|
---|---|---|---|
2 | 11 | emarinel | *
|
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 | 298 | emarinel | #include <signal.h> |
16 | 114 | emarinel | |
17 | 424 | emarinel | #include <ColonetServer.h> |
18 | 114 | emarinel | #include <wireless.h> // Colonet wireless library. |
19 | 297 | emarinel | #include <wl_token_ring.h> |
20 | 114 | emarinel | |
21 | 397 | emarinel | #include <colonet_defs.h> |
22 | #include <colonet_wireless.h> |
||
23 | 11 | emarinel | |
24 | /******************************* Definitions *********************************/
|
||
25 | |||
26 | 114 | emarinel | //Enable debug printouts
|
27 | 938 | rcahoon | //#define DEBUG 1
|
28 | 11 | emarinel | |
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 | 424 | emarinel | extern ColonetServer colonet_server;
|
38 | |||
39 | 114 | emarinel | static bool logging_enabled; |
40 | static char log_filename[80]; |
||
41 | static pthread_t listener_thread;
|
||
42 | 117 | emarinel | static char wl_port[40]; |
43 | 114 | emarinel | |
44 | 11 | emarinel | /************************* Internal prototypes *******************************/
|
45 | static void* listen(void* args); |
||
46 | 114 | emarinel | static void timeout_handler(void); |
47 | static void handle_response(int frame, int received); |
||
48 | 348 | emarinel | static void handle_receive(char type, int source, unsigned char* packet, int len); |
49 | 114 | emarinel | static void unregister(void); |
50 | 115 | emarinel | static int log_packet(unsigned char* packet, int len); |
51 | 11 | emarinel | |
52 | /**************************** Public functions *******************************/
|
||
53 | 618 | jknichel | /**
|
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 | 424 | emarinel | int colonet_wl_init(char* wl_port_, char* log_filename_) { |
62 | 347 | emarinel | if (log_filename_ != NULL) { |
63 | 11 | emarinel | logging_enabled = true;
|
64 | 347 | emarinel | strcpy(log_filename, log_filename_); |
65 | 11 | emarinel | } |
66 | |||
67 | 117 | emarinel | strncpy(wl_port, wl_port_, 40);
|
68 | |||
69 | 399 | emarinel | wl_set_com_port(wl_port); |
70 | 303 | emarinel | |
71 | 623 | emarinel | fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);
|
72 | 399 | emarinel | if (wl_init() != 0) { |
73 | fprintf(stderr, "wl_init failed.\n");
|
||
74 | return -1; |
||
75 | } |
||
76 | |||
77 | 297 | emarinel | wl_token_ring_register(); |
78 | 303 | emarinel | |
79 | 623 | emarinel | fprintf(stderr, "Joining token ring...\n");
|
80 | 397 | emarinel | if (wl_token_ring_join() != 0) { |
81 | fprintf(stderr, "Failed to join token ring.\n");
|
||
82 | return -1; |
||
83 | } |
||
84 | 623 | emarinel | fprintf(stderr, "Joined token ring.\n");
|
85 | 397 | emarinel | |
86 | return 0; |
||
87 | 11 | emarinel | } |
88 | |||
89 | 618 | jknichel | /**
|
90 | * @brief This function will kill the thread that is listening for wireless messages
|
||
91 | */
|
||
92 | 114 | emarinel | void colonet_wl_kill_listener_thread() {
|
93 | pthread_kill(listener_thread, 9);
|
||
94 | 11 | emarinel | } |
95 | |||
96 | 618 | jknichel | /**
|
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 | 117 | emarinel | int colonet_wl_run_listener_thread() {
|
102 | 11 | emarinel | dbg_printf("Spawning listener thread...\n");
|
103 | |||
104 | 115 | emarinel | if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
105 | 11 | emarinel | perror("pthread_create");
|
106 | 117 | emarinel | return -1; |
107 | 11 | emarinel | } |
108 | 117 | emarinel | |
109 | return 0; |
||
110 | 11 | emarinel | } |
111 | |||
112 | 618 | jknichel | /**
|
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 | 424 | emarinel | int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code, |
124 | 620 | gtress | unsigned char* args) { |
125 | 938 | rcahoon | dbg_printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
|
126 | 175 | emarinel | |
127 | 620 | gtress | //create and set up new packet to be sent.
|
128 | 117 | emarinel | ColonetRobotServerPacket pkt; |
129 | pkt.client_id = client_source; |
||
130 | pkt.msg_code = msg_code; |
||
131 | 167 | emarinel | |
132 | 618 | jknichel | //copy over the args into the data portion of the packet
|
133 | 648 | emarinel | if (args != NULL) { |
134 | 938 | rcahoon | dbg_printf("data ");
|
135 | 648 | emarinel | for (int i = 0; i < PACKET_DATA_LEN; i++) { |
136 | 938 | rcahoon | dbg_printf("%u, ", args[i]);
|
137 | 648 | emarinel | pkt.data[i] = args[i]; |
138 | } |
||
139 | 938 | rcahoon | dbg_printf("\n");
|
140 | 117 | emarinel | } |
141 | |||
142 | 114 | emarinel | if (dest == GLOBAL_DEST) {
|
143 | 518 | emarinel | //printf("sending to global dest\n");
|
144 | 620 | gtress | int ret = wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
145 | sizeof(ColonetRobotServerPacket), 0); |
||
146 | if (ret != 0) { |
||
147 | fprintf(stderr, "%s: wl_send_global_packet failed.\n", __FUNCTION__);
|
||
148 | 408 | emarinel | return -1; |
149 | } |
||
150 | 11 | emarinel | } else {
|
151 | 518 | emarinel | //("sending to specific robot: %d.\n", dest);
|
152 | 620 | gtress | int ret = wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
153 | sizeof(ColonetRobotServerPacket), dest,
|
||
154 | COLONET_RESPONSE_PACKET_FRAME_ID); |
||
155 | if (ret != 0) { |
||
156 | fprintf(stderr, "%s: wl_send_robot_to_robot_global_packet failed.\n", __FUNCTION__);
|
||
157 | 418 | emarinel | return -1; |
158 | } |
||
159 | 11 | emarinel | } |
160 | 409 | emarinel | |
161 | return 0; |
||
162 | 114 | emarinel | } |
163 | 11 | emarinel | |
164 | 618 | jknichel | /**
|
165 | * @brief Used to get the number of robots in the token ring
|
||
166 | *
|
||
167 | * @return The result of wl_token_get_num_robots()
|
||
168 | */
|
||
169 | 297 | emarinel | int colonet_get_num_robots(void) { |
170 | return wl_token_get_num_robots();
|
||
171 | } |
||
172 | |||
173 | 618 | jknichel | /**
|
174 | * @brief Gets a list of the xbee ids of the ones in the token ring
|
||
175 | *
|
||
176 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
177 | *
|
||
178 | * @return A dynamically allocated piece of memory that contains the list of xbee ids.
|
||
179 | * The function calling this function must free this memory when done with it.
|
||
180 | */
|
||
181 | 298 | emarinel | int* colonet_get_xbee_ids(int* numrobots) { |
182 | 297 | emarinel | int num_robots = wl_token_get_num_robots();
|
183 | int* ids = (int*)malloc(num_robots * sizeof(int)); |
||
184 | |||
185 | wl_token_iterator_begin(); |
||
186 | |||
187 | 418 | emarinel | int i = 0; |
188 | 297 | emarinel | while (wl_token_iterator_has_next()) {
|
189 | 418 | emarinel | ids[i] = wl_token_iterator_next(); |
190 | i++; |
||
191 | 297 | emarinel | } |
192 | |||
193 | 298 | emarinel | *numrobots = num_robots; |
194 | 297 | emarinel | return ids;
|
195 | } |
||
196 | |||
197 | 618 | jknichel | /**
|
198 | * @brief Gets the sensor matrix from the wireless library
|
||
199 | *
|
||
200 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
201 | 620 | gtress | * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids. This memory is dynamically
|
202 | * allocated and should be freed by the calling process when it is done with it
|
||
203 | 618 | jknichel | *
|
204 | 620 | gtress | * @return The 2d matrix reprsenting the sensor matrix. This memory is dynamically allocated and should be freed by
|
205 | * the calling process when it is done with it
|
||
206 | 618 | jknichel | */
|
207 | 298 | emarinel | int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
208 | 297 | emarinel | int num_robots;
|
209 | int* ids = colonet_get_xbee_ids(&num_robots);
|
||
210 | |||
211 | int** m = (int**)malloc(num_robots * sizeof(int*)); |
||
212 | for (int i = 0; i < num_robots; i++) { |
||
213 | m[i] = (int*)malloc(num_robots * sizeof(int*)); |
||
214 | } |
||
215 | |||
216 | for (int i = 0; i < num_robots; i++) { |
||
217 | for (int j = 0; j < num_robots; j++) { |
||
218 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
219 | } |
||
220 | } |
||
221 | |||
222 | 298 | emarinel | *numrobots = num_robots; |
223 | *ids_ = ids; |
||
224 | 297 | emarinel | return m;
|
225 | } |
||
226 | |||
227 | 114 | emarinel | /**************************** Private functions ******************************/
|
228 | |||
229 | 629 | jknichel | //TODO: fill in the doxygen comments
|
230 | /**
|
||
231 | 710 | gtress | * @brief
|
232 | 629 | jknichel | *
|
233 | * @return Void
|
||
234 | */
|
||
235 | 114 | emarinel | static void timeout_handler() { |
236 | 938 | rcahoon | dbg_printf("colonet wireless - timeout!\n");
|
237 | 11 | emarinel | } |
238 | |||
239 | 629 | jknichel | //TODO: fill in the doxygen comments
|
240 | /**
|
||
241 | 710 | gtress | * @brief
|
242 | 629 | jknichel | *
|
243 | * @param frame
|
||
244 | * @param received
|
||
245 | *
|
||
246 | * @return Void
|
||
247 | */
|
||
248 | 114 | emarinel | static void handle_response(int frame, int received) { |
249 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
250 | frame = frame; |
||
251 | received = received; |
||
252 | 114 | emarinel | } |
253 | |||
254 | 629 | jknichel | /**
|
255 | * @brief When a packet is received via wireless, the data is passed to this function
|
||
256 | *
|
||
257 | 710 | gtress | * @param type
|
258 | 629 | jknichel | * @param source The robot the packet came from
|
259 | * @param data The data the robot sent
|
||
260 | * @param len The length of the data
|
||
261 | *
|
||
262 | * @return Void
|
||
263 | */
|
||
264 | 648 | emarinel | static void handle_receive(char type, int source_robot, unsigned char* data, int len) { |
265 | 620 | gtress | type = type; //TODO: This is just here to get rid of compiler warnings.
|
266 | 457 | jknichel | |
267 | 681 | emarinel | //printf("***Got packet from robot***\n");
|
268 | 441 | jknichel | |
269 | 424 | emarinel | ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
270 | 117 | emarinel | |
271 | 114 | emarinel | if (logging_enabled) {
|
272 | 424 | emarinel | log_packet(data, len); |
273 | 11 | emarinel | } |
274 | 114 | emarinel | |
275 | 618 | jknichel | //see if a robot wants the server to tell it where it is
|
276 | 444 | emarinel | if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
|
277 | /* Robot has requested its position. */
|
||
278 | int robot_x, robot_y;
|
||
279 | 518 | emarinel | PositionMonitor* pm = colonet_server.getPositionMonitor(); |
280 | int num_robots = pm->getNumVisibleRobots();
|
||
281 | 629 | jknichel | |
282 | 618 | jknichel | //ask the position monitor for where that robot is.
|
283 | 648 | emarinel | int ret = pm->getRobotPosition(source_robot, &robot_x, &robot_y);
|
284 | 518 | emarinel | if (ret != 0 && num_robots != 1) { |
285 | 710 | gtress | //DEBUG
|
286 | 938 | rcahoon | dbg_fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source_robot);
|
287 | 648 | emarinel | |
288 | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_ROBOT_LOST, NULL) != 0) { |
||
289 | fprintf(stderr, "colonet_wl_send failed!\n");
|
||
290 | exit(1);
|
||
291 | } |
||
292 | 444 | emarinel | } else {
|
293 | 648 | emarinel | if (ret != 0 && num_robots == 1) { |
294 | 518 | emarinel | VisionPosition pos = pm->getFirstPosition(); |
295 | robot_x = pos.x; |
||
296 | robot_y = pos.y; |
||
297 | } |
||
298 | |||
299 | 444 | emarinel | unsigned char response[80]; |
300 | 518 | emarinel | response[0] = (robot_x >> 8) & 0xFF; |
301 | response[1] = robot_x & 0xFF; |
||
302 | response[2] = (robot_y >> 8) & 0xFF; |
||
303 | response[3] = robot_y & 0xFF; |
||
304 | 117 | emarinel | |
305 | 938 | rcahoon | dbg_fprintf(stderr, "Robot %d requested position. Its position is %d,%d.\n", source_robot, robot_x, robot_y);
|
306 | 518 | emarinel | |
307 | 648 | emarinel | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
308 | 444 | emarinel | fprintf(stderr, "colonet_wl_send failed!\n");
|
309 | exit(1);
|
||
310 | } |
||
311 | } |
||
312 | } else {
|
||
313 | 938 | rcahoon | dbg_printf("Calling colonet_server.process_received_wireless_message\n");
|
314 | 444 | emarinel | char processed_data[80]; |
315 | 648 | emarinel | sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source_robot, pkt->data);
|
316 | 424 | emarinel | |
317 | 938 | rcahoon | dbg_fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);
|
318 | 623 | emarinel | |
319 | 444 | emarinel | colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
320 | } |
||
321 | 11 | emarinel | } |
322 | |||
323 | 629 | jknichel | //TODO: fill in the doxygen comments
|
324 | /**
|
||
325 | 710 | gtress | * @brief
|
326 | 629 | jknichel | *
|
327 | * @return Void
|
||
328 | */
|
||
329 | 114 | emarinel | static void unregister(void) { |
330 | printf("unregister\n");
|
||
331 | } |
||
332 | 11 | emarinel | |
333 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
334 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
335 | * Not part of the ColonetWireless class since a function pointer must be
|
||
336 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
337 | 11 | emarinel | * a class function).
|
338 | *
|
||
339 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
340 | * @return NULL
|
||
341 | */
|
||
342 | 114 | emarinel | static void* listen(void* args) { |
343 | 620 | gtress | args = args; //TODO: These are just here to get rid of compiler warnings.
|
344 | 457 | jknichel | |
345 | 620 | gtress | fprintf(stderr, "Called listen.\n");
|
346 | 303 | emarinel | |
347 | 620 | gtress | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister}; |
348 | 114 | emarinel | wl_register_packet_group(&pgh); |
349 | 11 | emarinel | |
350 | 114 | emarinel | while (1) { |
351 | wl_do(); |
||
352 | 629 | jknichel | |
353 | //this sleep is here so the thread this runs in doesn't hog the cpu
|
||
354 | 409 | emarinel | usleep(1000);
|
355 | 11 | emarinel | } |
356 | |||
357 | 623 | emarinel | fprintf(stderr, "Listen is returning.\n");
|
358 | |||
359 | 114 | emarinel | wl_terminate(); |
360 | 11 | emarinel | return NULL; |
361 | } |
||
362 | |||
363 | 710 | gtress | /**
|
364 | 629 | jknichel | * @brief
|
365 | 11 | emarinel | *
|
366 | * @param pkt Packet to be logged
|
||
367 | *
|
||
368 | * @return 0 on success, -1 on failure
|
||
369 | */
|
||
370 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
371 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
372 | |||
373 | if (logfile == NULL) { |
||
374 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
375 | return -1; |
||
376 | } |
||
377 | |||
378 | 114 | emarinel | for (int i = 0; i < len; i++) { |
379 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
380 | 11 | emarinel | } |
381 | fprintf(logfile, "\n");
|
||
382 | |||
383 | fclose(logfile); |
||
384 | return 0; |
||
385 | } |