root / trunk / code / projects / colonet / ColonetServer / colonet_wireless.cpp @ 392
History | View | Annotate | Download (5.91 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 | #include <wireless.h> // Colonet wireless library. |
||
18 | 297 | emarinel | #include <wl_token_ring.h> |
19 | 114 | emarinel | |
20 | #include "colonet_defs.h" |
||
21 | 11 | emarinel | #include "colonet_wireless.h" |
22 | |||
23 | /******************************* Definitions *********************************/
|
||
24 | |||
25 | 114 | emarinel | //Enable debug printouts
|
26 | 11 | emarinel | #define DEBUG 1 |
27 | |||
28 | #ifdef DEBUG
|
||
29 | #define dbg_printf(...) printf(__VA_ARGS__)
|
||
30 | #define dbg_fprintf(...) fprintf(__VA_ARGS__)
|
||
31 | #else
|
||
32 | #define dbg_printf(...)
|
||
33 | #define dbg_fprintf(...)
|
||
34 | #endif
|
||
35 | |||
36 | 114 | emarinel | static MsgHandlerFunction message_handler;
|
37 | static bool logging_enabled; |
||
38 | static char log_filename[80]; |
||
39 | static pthread_t listener_thread;
|
||
40 | 117 | emarinel | static char wl_port[40]; |
41 | 114 | emarinel | |
42 | 11 | emarinel | /************************* Internal prototypes *******************************/
|
43 | static void* listen(void* args); |
||
44 | 114 | emarinel | static void timeout_handler(void); |
45 | static void handle_response(int frame, int received); |
||
46 | 348 | emarinel | static void handle_receive(char type, int source, unsigned char* packet, int len); |
47 | 114 | emarinel | static void unregister(void); |
48 | 115 | emarinel | static int log_packet(unsigned char* packet, int len); |
49 | 11 | emarinel | |
50 | /**************************** Public functions *******************************/
|
||
51 | 347 | emarinel | void colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) { |
52 | if (log_filename_ != NULL) { |
||
53 | 11 | emarinel | logging_enabled = true;
|
54 | 347 | emarinel | strcpy(log_filename, log_filename_); |
55 | 11 | emarinel | } |
56 | |||
57 | 117 | emarinel | strncpy(wl_port, wl_port_, 40);
|
58 | |||
59 | 303 | emarinel | message_handler = message_handler_; |
60 | |||
61 | printf("Calling wl_init...\n");
|
||
62 | wl_init(); |
||
63 | printf("wl_init succeeded.\n");
|
||
64 | |||
65 | 297 | emarinel | wl_token_ring_register(); |
66 | 303 | emarinel | |
67 | printf("Joining token ring...\n");
|
||
68 | 297 | emarinel | wl_token_ring_join(); |
69 | 303 | emarinel | printf("Joined token ring.\n");
|
70 | 11 | emarinel | } |
71 | |||
72 | 114 | emarinel | void colonet_wl_kill_listener_thread() {
|
73 | pthread_kill(listener_thread, 9);
|
||
74 | 11 | emarinel | } |
75 | |||
76 | 117 | emarinel | int colonet_wl_run_listener_thread() {
|
77 | 11 | emarinel | dbg_printf("Spawning listener thread...\n");
|
78 | |||
79 | 115 | emarinel | if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
80 | 11 | emarinel | perror("pthread_create");
|
81 | 117 | emarinel | return -1; |
82 | 11 | emarinel | } |
83 | 117 | emarinel | |
84 | return 0; |
||
85 | 11 | emarinel | } |
86 | |||
87 | 297 | emarinel | void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code, |
88 | 117 | emarinel | unsigned char* args) { |
89 | 297 | emarinel | printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
|
90 | 175 | emarinel | |
91 | 117 | emarinel | ColonetRobotServerPacket pkt; |
92 | pkt.client_id = client_source; |
||
93 | pkt.msg_code = msg_code; |
||
94 | 167 | emarinel | |
95 | 117 | emarinel | for (int i = 0; i < PACKET_DATA_LEN; i++) { |
96 | pkt.data[i] = args[i]; |
||
97 | } |
||
98 | |||
99 | 114 | emarinel | if (dest == GLOBAL_DEST) {
|
100 | 174 | emarinel | printf("sending to global dest\n");
|
101 | 175 | emarinel | |
102 | 297 | emarinel | wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
103 | 11 | emarinel | } else {
|
104 | 174 | emarinel | printf("sending to specific robot.\n");
|
105 | 297 | emarinel | wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
106 | sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID);
|
||
107 | 11 | emarinel | } |
108 | 114 | emarinel | } |
109 | 11 | emarinel | |
110 | 297 | emarinel | int colonet_get_num_robots(void) { |
111 | return wl_token_get_num_robots();
|
||
112 | } |
||
113 | |||
114 | 298 | emarinel | int* colonet_get_xbee_ids(int* numrobots) { |
115 | 297 | emarinel | int num_robots = wl_token_get_num_robots();
|
116 | int* ids = (int*)malloc(num_robots * sizeof(int)); |
||
117 | |||
118 | wl_token_iterator_begin(); |
||
119 | |||
120 | int* p = ids;
|
||
121 | while (wl_token_iterator_has_next()) {
|
||
122 | *p = wl_token_iterator_next(); |
||
123 | p++; |
||
124 | } |
||
125 | |||
126 | 298 | emarinel | *numrobots = num_robots; |
127 | 297 | emarinel | return ids;
|
128 | } |
||
129 | |||
130 | // Returns int**; should be freed
|
||
131 | 298 | emarinel | int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
132 | 297 | emarinel | int num_robots;
|
133 | int* ids = colonet_get_xbee_ids(&num_robots);
|
||
134 | |||
135 | int** m = (int**)malloc(num_robots * sizeof(int*)); |
||
136 | for (int i = 0; i < num_robots; i++) { |
||
137 | m[i] = (int*)malloc(num_robots * sizeof(int*)); |
||
138 | } |
||
139 | |||
140 | for (int i = 0; i < num_robots; i++) { |
||
141 | for (int j = 0; j < num_robots; j++) { |
||
142 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
143 | } |
||
144 | } |
||
145 | |||
146 | 298 | emarinel | *numrobots = num_robots; |
147 | *ids_ = ids; |
||
148 | 297 | emarinel | return m;
|
149 | } |
||
150 | |||
151 | 114 | emarinel | /**************************** Private functions ******************************/
|
152 | |||
153 | static void timeout_handler() { |
||
154 | 297 | emarinel | printf("colonet wireless - timeout!\n");
|
155 | 11 | emarinel | } |
156 | |||
157 | 114 | emarinel | static void handle_response(int frame, int received) { |
158 | printf("handle response\n");
|
||
159 | } |
||
160 | |||
161 | static void handle_receive(char type, int source, unsigned char* packet, |
||
162 | int len) {
|
||
163 | 117 | emarinel | ColonetRobotServerPacket pkt; |
164 | 348 | emarinel | pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) | (packet[3]<<24); //little endian |
165 | 117 | emarinel | |
166 | memcpy(pkt.data, packet + 5, PACKET_DATA_LEN);
|
||
167 | |||
168 | 114 | emarinel | if (logging_enabled) {
|
169 | 115 | emarinel | log_packet(packet, len); |
170 | 11 | emarinel | } |
171 | 114 | emarinel | |
172 | 117 | emarinel | message_handler(type, source, pkt.client_id, packet, len); |
173 | |||
174 | 114 | emarinel | printf("handle receive\n");
|
175 | 11 | emarinel | } |
176 | |||
177 | 114 | emarinel | static void unregister(void) { |
178 | printf("unregister\n");
|
||
179 | } |
||
180 | 11 | emarinel | |
181 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
182 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
183 | * Not part of the ColonetWireless class since a function pointer must be
|
||
184 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
185 | 11 | emarinel | * a class function).
|
186 | *
|
||
187 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
188 | * @return NULL
|
||
189 | */
|
||
190 | 114 | emarinel | static void* listen(void* args) { |
191 | 174 | emarinel | printf("Called listen.\n");
|
192 | 303 | emarinel | |
193 | 114 | emarinel | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, |
194 | timeout_handler, |
||
195 | handle_response, |
||
196 | handle_receive, |
||
197 | unregister}; |
||
198 | wl_register_packet_group(&pgh); |
||
199 | 11 | emarinel | |
200 | 114 | emarinel | while (1) { |
201 | wl_do(); |
||
202 | 11 | emarinel | } |
203 | |||
204 | 114 | emarinel | wl_terminate(); |
205 | 11 | emarinel | pthread_exit(NULL);
|
206 | return NULL; |
||
207 | } |
||
208 | |||
209 | 114 | emarinel | /** @brief
|
210 | 11 | emarinel | *
|
211 | * @param pkt Packet to be logged
|
||
212 | *
|
||
213 | * @return 0 on success, -1 on failure
|
||
214 | */
|
||
215 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
216 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
217 | |||
218 | if (logfile == NULL) { |
||
219 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
220 | return -1; |
||
221 | } |
||
222 | |||
223 | 114 | emarinel | for (int i = 0; i < len; i++) { |
224 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
225 | 11 | emarinel | } |
226 | fprintf(logfile, "\n");
|
||
227 | |||
228 | fclose(logfile); |
||
229 | return 0; |
||
230 | } |