root / branches / colonetmk2 / code / projects / swarm / colonet_wireless.cpp @ 1456
History | View | Annotate | Download (4.54 KB)
1 |
/** @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 |
} |
175 |
|