root / branches / colonetmk2 / code / projects / swarm / colonet_wireless.cpp @ 1456
History | View | Annotate | Download (4.54 KB)
1 | 1456 | rcahoon | /** @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 <stdio.h> |
||
10 | #include <string.h> |
||
11 | #include <stdlib.h> |
||
12 | #include <unistd.h> |
||
13 | #include <pthread.h> |
||
14 | #include <signal.h> |
||
15 | |||
16 | #include <wireless.h> // Colonet wireless library. |
||
17 | #include <wl_token_ring.h> |
||
18 | |||
19 | #include <colonet_wireless.h> |
||
20 | |||
21 | /******************************* Definitions *********************************/
|
||
22 | |||
23 | //Enable debug printouts
|
||
24 | //#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 | static pthread_t listener_thread;
|
||
35 | static char wl_port[40]; |
||
36 | |||
37 | /************************* Internal prototypes *******************************/
|
||
38 | static void* listen(void* args); |
||
39 | |||
40 | /**************************** Public functions *******************************/
|
||
41 | /**
|
||
42 | * @brief Initializes the wireless library
|
||
43 | *
|
||
44 | * @param wl_port_ The port to listen for wireless messages on
|
||
45 | *
|
||
46 | * @return 0 on success, negative error code on failure
|
||
47 | */
|
||
48 | int colonet_wl_init(char* wl_port_) { |
||
49 | strncpy(wl_port, wl_port_, 40);
|
||
50 | |||
51 | wl_set_com_port(wl_port); |
||
52 | |||
53 | fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);
|
||
54 | if (wl_init() != 0) { |
||
55 | fprintf(stderr, "wl_init failed.\n");
|
||
56 | return -1; |
||
57 | } |
||
58 | |||
59 | fprintf(stderr, "Setting channel\n");
|
||
60 | wl_set_channel(0xF);
|
||
61 | wl_set_pan(0x3332);
|
||
62 | |||
63 | /*wl_token_ring_register();
|
||
64 | |||
65 | fprintf(stderr, "Joining token ring...\n");
|
||
66 | if (wl_token_ring_join() != 0) {
|
||
67 | fprintf(stderr, "Failed to join token ring.\n");
|
||
68 | return -1;
|
||
69 | }
|
||
70 | fprintf(stderr, "Joined token ring.\n");*/
|
||
71 | |||
72 | return 0; |
||
73 | } |
||
74 | |||
75 | /**
|
||
76 | * @brief This function will kill the thread that is listening for wireless messages
|
||
77 | */
|
||
78 | void colonet_wl_kill_listener_thread() {
|
||
79 | pthread_kill(listener_thread, 9);
|
||
80 | } |
||
81 | |||
82 | void colonet_wl_join() {
|
||
83 | pthread_join(listener_thread, NULL);
|
||
84 | } |
||
85 | |||
86 | /**
|
||
87 | * @brief This function spawns a thread to listen for wireless messages
|
||
88 | *
|
||
89 | * @return 0 on success, negative error code on failure
|
||
90 | */
|
||
91 | int colonet_wl_run_listener_thread() {
|
||
92 | dbg_printf("Spawning listener thread...\n");
|
||
93 | |||
94 | if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
||
95 | perror("pthread_create");
|
||
96 | return -1; |
||
97 | } |
||
98 | |||
99 | return 0; |
||
100 | } |
||
101 | |||
102 | /**
|
||
103 | * @brief Used to get the number of robots in the token ring
|
||
104 | *
|
||
105 | * @return The result of wl_token_get_num_robots()
|
||
106 | */
|
||
107 | int colonet_get_num_robots(void) { |
||
108 | return wl_token_get_num_robots();
|
||
109 | } |
||
110 | |||
111 | /**
|
||
112 | * @brief Gets a list of the xbee ids of the ones in the token ring
|
||
113 | *
|
||
114 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
115 | *
|
||
116 | * @return A dynamically allocated piece of memory that contains the list of xbee ids.
|
||
117 | * The function calling this function must free this memory when done with it.
|
||
118 | */
|
||
119 | void colonet_get_xbee_ids(int ids[]) { |
||
120 | wl_token_iterator_begin(); |
||
121 | |||
122 | int i = 0; |
||
123 | while (wl_token_iterator_has_next()) {
|
||
124 | ids[i++] = wl_token_iterator_next(); |
||
125 | } |
||
126 | } |
||
127 | |||
128 | /**
|
||
129 | * @brief Gets the sensor matrix from the wireless library
|
||
130 | *
|
||
131 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
132 | * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids. This memory is dynamically
|
||
133 | * allocated and should be freed by the calling process when it is done with it
|
||
134 | *
|
||
135 | * @return The 2d matrix reprsenting the sensor matrix. This memory is dynamically allocated and should be freed by
|
||
136 | * the calling process when it is done with it
|
||
137 | */
|
||
138 | void colonet_get_sensor_matrix(int* ids, SensorReading*** m) { |
||
139 | int num_robots = colonet_get_num_robots();
|
||
140 | colonet_get_xbee_ids(ids); |
||
141 | |||
142 | for (int i = 0; i < num_robots; i++) { |
||
143 | for (int j = 0; j < num_robots; j++) { |
||
144 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
145 | } |
||
146 | } |
||
147 | } |
||
148 | |||
149 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
150 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
151 | * Not part of the ColonetWireless class since a function pointer must be
|
||
152 | * passed in starting the thread that runs it (tricky or impossible if it's
|
||
153 | * a class function).
|
||
154 | *
|
||
155 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
156 | * @return NULL
|
||
157 | */
|
||
158 | static void* listen(void* args) { |
||
159 | args = args; //TODO: These are just here to get rid of compiler warnings.
|
||
160 | |||
161 | fprintf(stderr, "Called listen.\n");
|
||
162 | |||
163 | while (1) { |
||
164 | wl_do(); |
||
165 | |||
166 | //this sleep is here so the thread this runs in doesn't hog the cpu
|
||
167 | pthread_yield(); |
||
168 | } |
||
169 | |||
170 | fprintf(stderr, "Listen is returning.\n");
|
||
171 | |||
172 | wl_terminate(); |
||
173 | return NULL; |
||
174 | } |