root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 482
History | View | Annotate | Download (7.11 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 | 11 | emarinel | #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 | 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 | 424 | emarinel | int colonet_wl_init(char* wl_port_, char* log_filename_) { |
54 | 347 | emarinel | if (log_filename_ != NULL) { |
55 | 11 | emarinel | logging_enabled = true;
|
56 | 347 | emarinel | strcpy(log_filename, log_filename_); |
57 | 11 | emarinel | } |
58 | |||
59 | 117 | emarinel | strncpy(wl_port, wl_port_, 40);
|
60 | |||
61 | 399 | emarinel | wl_set_com_port(wl_port); |
62 | 303 | emarinel | |
63 | 399 | emarinel | printf("Calling wl_init(%s)...\n", wl_port);
|
64 | if (wl_init() != 0) { |
||
65 | fprintf(stderr, "wl_init failed.\n");
|
||
66 | return -1; |
||
67 | } |
||
68 | |||
69 | 297 | emarinel | wl_token_ring_register(); |
70 | 303 | emarinel | |
71 | printf("Joining token ring...\n");
|
||
72 | 397 | emarinel | if (wl_token_ring_join() != 0) { |
73 | fprintf(stderr, "Failed to join token ring.\n");
|
||
74 | return -1; |
||
75 | } |
||
76 | 303 | emarinel | printf("Joined token ring.\n");
|
77 | 397 | emarinel | |
78 | return 0; |
||
79 | 11 | emarinel | } |
80 | |||
81 | 114 | emarinel | void colonet_wl_kill_listener_thread() {
|
82 | pthread_kill(listener_thread, 9);
|
||
83 | 11 | emarinel | } |
84 | |||
85 | 117 | emarinel | int colonet_wl_run_listener_thread() {
|
86 | 11 | emarinel | dbg_printf("Spawning listener thread...\n");
|
87 | |||
88 | 115 | emarinel | if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
89 | 11 | emarinel | perror("pthread_create");
|
90 | 117 | emarinel | return -1; |
91 | 11 | emarinel | } |
92 | 117 | emarinel | |
93 | return 0; |
||
94 | 11 | emarinel | } |
95 | |||
96 | 424 | emarinel | int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code, |
97 | unsigned char* args) { |
||
98 | 297 | emarinel | printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
|
99 | 175 | emarinel | |
100 | 117 | emarinel | ColonetRobotServerPacket pkt; |
101 | pkt.client_id = client_source; |
||
102 | pkt.msg_code = msg_code; |
||
103 | 167 | emarinel | |
104 | 117 | emarinel | for (int i = 0; i < PACKET_DATA_LEN; i++) { |
105 | pkt.data[i] = args[i]; |
||
106 | } |
||
107 | |||
108 | 114 | emarinel | if (dest == GLOBAL_DEST) {
|
109 | 174 | emarinel | printf("sending to global dest\n");
|
110 | 408 | emarinel | if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
111 | sizeof(ColonetRobotServerPacket), 0) != 0) { |
||
112 | return -1; |
||
113 | } |
||
114 | 11 | emarinel | } else {
|
115 | 424 | emarinel | printf("sending to specific robot: %d.\n", dest);
|
116 | 418 | emarinel | if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
117 | sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) { |
||
118 | return -1; |
||
119 | } |
||
120 | 11 | emarinel | } |
121 | 409 | emarinel | |
122 | return 0; |
||
123 | 114 | emarinel | } |
124 | 11 | emarinel | |
125 | 297 | emarinel | int colonet_get_num_robots(void) { |
126 | return wl_token_get_num_robots();
|
||
127 | } |
||
128 | |||
129 | 298 | emarinel | int* colonet_get_xbee_ids(int* numrobots) { |
130 | 297 | emarinel | int num_robots = wl_token_get_num_robots();
|
131 | int* ids = (int*)malloc(num_robots * sizeof(int)); |
||
132 | |||
133 | wl_token_iterator_begin(); |
||
134 | |||
135 | 418 | emarinel | int i = 0; |
136 | 297 | emarinel | while (wl_token_iterator_has_next()) {
|
137 | 418 | emarinel | ids[i] = wl_token_iterator_next(); |
138 | i++; |
||
139 | 297 | emarinel | } |
140 | |||
141 | 298 | emarinel | *numrobots = num_robots; |
142 | 297 | emarinel | return ids;
|
143 | } |
||
144 | |||
145 | // Returns int**; should be freed
|
||
146 | 298 | emarinel | int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
147 | 297 | emarinel | int num_robots;
|
148 | int* ids = colonet_get_xbee_ids(&num_robots);
|
||
149 | |||
150 | int** m = (int**)malloc(num_robots * sizeof(int*)); |
||
151 | for (int i = 0; i < num_robots; i++) { |
||
152 | m[i] = (int*)malloc(num_robots * sizeof(int*)); |
||
153 | } |
||
154 | |||
155 | for (int i = 0; i < num_robots; i++) { |
||
156 | for (int j = 0; j < num_robots; j++) { |
||
157 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
158 | } |
||
159 | } |
||
160 | |||
161 | 298 | emarinel | *numrobots = num_robots; |
162 | *ids_ = ids; |
||
163 | 297 | emarinel | return m;
|
164 | } |
||
165 | |||
166 | 114 | emarinel | /**************************** Private functions ******************************/
|
167 | |||
168 | static void timeout_handler() { |
||
169 | 418 | emarinel | // printf("colonet wireless - timeout!\n");
|
170 | 11 | emarinel | } |
171 | |||
172 | 114 | emarinel | static void handle_response(int frame, int received) { |
173 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
174 | frame = frame; |
||
175 | received = received; |
||
176 | 424 | emarinel | //printf("got response.\n");
|
177 | 114 | emarinel | } |
178 | |||
179 | 424 | emarinel | static void handle_receive(char type, int source, unsigned char* data, int len) { |
180 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
181 | type = type; |
||
182 | |||
183 | 444 | emarinel | printf("handle receive\n");
|
184 | 441 | jknichel | |
185 | 424 | emarinel | ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
186 | 117 | emarinel | |
187 | 114 | emarinel | if (logging_enabled) {
|
188 | 424 | emarinel | log_packet(data, len); |
189 | 11 | emarinel | } |
190 | 114 | emarinel | |
191 | 444 | emarinel | if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
|
192 | /* Robot has requested its position. */
|
||
193 | int robot_x, robot_y;
|
||
194 | if (colonet_server.getPositionMonitor()->getRobotPosition(source, &robot_x, &robot_y) != 0) { |
||
195 | fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);
|
||
196 | } else {
|
||
197 | unsigned char response[80]; |
||
198 | response[0] = robot_x & 0xFF; |
||
199 | response[1] = (robot_x >> 8) & 0xFF; |
||
200 | response[2] = robot_y & 0xFF; |
||
201 | response[3] = (robot_y >> 8) & 0xFF; |
||
202 | 117 | emarinel | |
203 | 444 | emarinel | if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
204 | fprintf(stderr, "colonet_wl_send failed!\n");
|
||
205 | exit(1);
|
||
206 | } |
||
207 | } |
||
208 | } else {
|
||
209 | char processed_data[80]; |
||
210 | sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data);
|
||
211 | 424 | emarinel | |
212 | 444 | emarinel | colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
213 | } |
||
214 | 11 | emarinel | } |
215 | |||
216 | 114 | emarinel | static void unregister(void) { |
217 | printf("unregister\n");
|
||
218 | } |
||
219 | 11 | emarinel | |
220 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
221 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
222 | * Not part of the ColonetWireless class since a function pointer must be
|
||
223 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
224 | 11 | emarinel | * a class function).
|
225 | *
|
||
226 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
227 | * @return NULL
|
||
228 | */
|
||
229 | 114 | emarinel | static void* listen(void* args) { |
230 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
231 | args = args; |
||
232 | |||
233 | 174 | emarinel | printf("Called listen.\n");
|
234 | 303 | emarinel | |
235 | 114 | emarinel | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, |
236 | timeout_handler, |
||
237 | handle_response, |
||
238 | handle_receive, |
||
239 | unregister}; |
||
240 | wl_register_packet_group(&pgh); |
||
241 | 11 | emarinel | |
242 | 114 | emarinel | while (1) { |
243 | wl_do(); |
||
244 | 409 | emarinel | usleep(1000);
|
245 | 11 | emarinel | } |
246 | |||
247 | 114 | emarinel | wl_terminate(); |
248 | 11 | emarinel | return NULL; |
249 | } |
||
250 | |||
251 | 114 | emarinel | /** @brief
|
252 | 11 | emarinel | *
|
253 | * @param pkt Packet to be logged
|
||
254 | *
|
||
255 | * @return 0 on success, -1 on failure
|
||
256 | */
|
||
257 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
258 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
259 | |||
260 | if (logfile == NULL) { |
||
261 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
262 | return -1; |
||
263 | } |
||
264 | |||
265 | 114 | emarinel | for (int i = 0; i < len; i++) { |
266 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
267 | 11 | emarinel | } |
268 | fprintf(logfile, "\n");
|
||
269 | |||
270 | fclose(logfile); |
||
271 | return 0; |
||
272 | } |