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