root / trunk / code / projects / mapping / matlab / receive.c @ 960
History | View | Annotate | Download (6.24 KB)
1 | 952 | justin | #include <stdio.h> |
---|---|---|---|
2 | #include <stdlib.h> |
||
3 | 959 | justin | #include <mex.h> |
4 | 952 | justin | #include <pthread.h> |
5 | #include <unistd.h> |
||
6 | #include "../../libwireless/lib/wireless.h" |
||
7 | #include "../../libwireless/lib/wl_token_ring.h" |
||
8 | |||
9 | #define WL_COM_PORT "/dev/ttyUSB1" |
||
10 | #define WL_CHANNEL 0xE |
||
11 | |||
12 | 960 | justin | #define MAP_RECEIVE_GROUP 17 |
13 | #define MAP_REQUEST_GROUP 23 |
||
14 | 952 | justin | |
15 | 959 | justin | #define POINT_RAW 1 /* packet type for map data points w/ raw encoder data*/ |
16 | #define POINT_ODO 2 /* packet type for map data points w/ odometry data*/ |
||
17 | 952 | justin | |
18 | #define BUFFER_SIZE 1000 |
||
19 | #define BUFFER_EMPTY -1 |
||
20 | |||
21 | pthread_rwlock_t buffer_lock; |
||
22 | pthread_t receive_thread; |
||
23 | |||
24 | 959 | justin | int packets_received; /*Used to block overlapping packet requests.*/ |
25 | 952 | justin | |
26 | void packet_receive(char type, int source, unsigned char* packet, int length); |
||
27 | |||
28 | PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL}; |
||
29 | PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL}; |
||
30 | |||
31 | 959 | justin | /*Buffers*/
|
32 | short robot_buffer[BUFFER_SIZE]; /*by #*/ |
||
33 | short range_buffer[BUFFER_SIZE][5]; /*IR1, IR2 ... */ |
||
34 | short coordinate_buffer[BUFFER_SIZE][2]; /*X Y*/ |
||
35 | 952 | justin | float orientation_buffer[BUFFER_SIZE];
|
36 | |||
37 | 959 | justin | int tail_index; /*Points to the newest data in the buffers.*/ |
38 | int head_index; /*Points to the oldest data in the buffers.*/ |
||
39 | 952 | justin | |
40 | void* buffer_data(void* arg); |
||
41 | |||
42 | 959 | justin | void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){ |
43 | |||
44 | if(nrhs == 1){ |
||
45 | 952 | justin | |
46 | 959 | justin | /*Initialization*/
|
47 | 952 | justin | head_index = BUFFER_EMPTY; |
48 | tail_index = BUFFER_EMPTY; |
||
49 | |||
50 | pthread_rwlock_init(&buffer_lock, NULL);
|
||
51 | |||
52 | pthread_create(&receive_thread, NULL, buffer_data, NULL); |
||
53 | 959 | justin | return;
|
54 | 952 | justin | } |
55 | |||
56 | 959 | justin | /*Return stuff to matlab*/
|
57 | /*deliberately ignore nrhs. */
|
||
58 | |||
59 | 952 | justin | if(nlhs != 4){ |
60 | 959 | justin | mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n \
|
61 | where P = [X Y], IR = [IR1, IR2 ...],\n \
|
||
62 | 952 | justin | T = angle, and R = Robot number.");
|
63 | } |
||
64 | |||
65 | int index,i,j;
|
||
66 | |||
67 | pthread_rwlock_rdlock(&buffer_lock); |
||
68 | { |
||
69 | |||
70 | if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
|
||
71 | 959 | justin | double* point, *ranges, *theta, *robot;
|
72 | /*Appropriately return with nothing.*/
|
||
73 | ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL); |
||
74 | ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL); |
||
75 | ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL); |
||
76 | ret[3] = mxCreateDoubleMatrix(1, 1, mxREAL); |
||
77 | |||
78 | point = mxGetPr(ret[0]);
|
||
79 | ranges = mxGetPr(ret[1]);
|
||
80 | theta = mxGetPr(ret[2]);
|
||
81 | robot = mxGetPr(ret[3]);
|
||
82 | |||
83 | point[0] = point[1] = 0.0; |
||
84 | |||
85 | ranges[0] = ranges[1] = ranges[2] = 0.0; |
||
86 | ranges[3] = ranges[4] = 0.0; |
||
87 | |||
88 | theta[0] = 0.0; |
||
89 | robot[0] = 0.0; |
||
90 | |||
91 | return;
|
||
92 | 952 | justin | } |
93 | else{
|
||
94 | 959 | justin | double** point, **ranges, *theta, *robot;
|
95 | 960 | justin | |
96 | 952 | justin | int num_buffer_elements = tail_index - head_index;
|
97 | if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE; |
||
98 | |||
99 | ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL); |
||
100 | ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL); |
||
101 | ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
||
102 | ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
||
103 | |||
104 | 959 | justin | /*This may not work on some computers.*/
|
105 | point = (double**)mxGetPr(ret[0]); |
||
106 | ranges = (double**)mxGetPr(ret[1]); |
||
107 | 952 | justin | theta = mxGetPr(ret[2]);
|
108 | robot = mxGetPr(ret[3]);
|
||
109 | 959 | justin | |
110 | 952 | justin | for(index = head_index; index <= tail_index; index = (index + 1)%1024){ |
111 | point[i][0] = (double)coordinate_buffer[index][0]; |
||
112 | point[i][1] = (double)coordinate_buffer[index][1]; |
||
113 | |||
114 | for(j=0;j<5;j++) |
||
115 | ranges[i][j] = (double)range_buffer[index][j];
|
||
116 | |||
117 | theta[i] = (double)(orientation_buffer[index]);
|
||
118 | robot[i] = (double)(robot_buffer[index]);
|
||
119 | } |
||
120 | head_index = BUFFER_EMPTY; |
||
121 | tail_index = BUFFER_EMPTY; |
||
122 | 959 | justin | } |
123 | 952 | justin | } |
124 | pthread_rwlock_unlock(&buffer_lock); |
||
125 | } |
||
126 | |||
127 | /*
|
||
128 | * buffer_data continuously runs through the robots and
|
||
129 | * waits for them to respond.
|
||
130 | */
|
||
131 | void* buffer_data(void* arg){ |
||
132 | int robot_num, num_robots;
|
||
133 | |||
134 | pthread_detach(pthread_self()); |
||
135 | |||
136 | 959 | justin | /*General communication initialization. */
|
137 | 952 | justin | wl_set_com_port(WL_COM_PORT); |
138 | wl_init(); |
||
139 | wl_set_channel(WL_CHANNEL); |
||
140 | wl_register_packet_group(&receiveHandler); |
||
141 | |||
142 | 959 | justin | /*Token ring / sensor network initialization.*/
|
143 | 952 | justin | wl_token_ring_register(); |
144 | |||
145 | 959 | justin | /*Continuously iterate over all the robots - buffering data.*/
|
146 | 960 | justin | |
147 | 952 | justin | while(1){ |
148 | |||
149 | wl_token_iterator_begin(); |
||
150 | |||
151 | num_robots = wl_token_get_num_robots(); |
||
152 | |||
153 | 959 | justin | /*Initialize the 'packets received' number. */
|
154 | 952 | justin | packets_received = 0;
|
155 | |||
156 | 959 | justin | /*Send a data request to all robots.*/
|
157 | 952 | justin | while(wl_token_iterator_has_next()){
|
158 | robot_num = wl_token_iterator_next(); |
||
159 | |||
160 | 953 | justin | wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
161 | POINT_ODO, NULL, 0, robot_num, 0); |
||
162 | |||
163 | 960 | justin | usleep(10000); /*10ms*/ |
164 | 952 | justin | } |
165 | |||
166 | 960 | justin | /*Wait for all packets to be received
|
167 | * before sending more requests.
|
||
168 | * FIXME this needs to be WAAAAY more robust
|
||
169 | * to edge cases (robot enters / leaves token
|
||
170 | * ring etc.) For example, if a robot drops
|
||
171 | * a packet (highly likely) it is not dealt with
|
||
172 | * */
|
||
173 | 952 | justin | while(packets_received < num_robots
|
174 | && packets_received < wl_token_get_num_robots()){ |
||
175 | |||
176 | 959 | justin | usleep(10000); /*10ms*/ |
177 | 952 | justin | } |
178 | |||
179 | usleep(10000);
|
||
180 | } |
||
181 | } |
||
182 | |||
183 | void packet_receive (char type, int source, unsigned char* packet, int length) { |
||
184 | printf("A packet came in!\n");
|
||
185 | if(type==POINT_ODO){
|
||
186 | |||
187 | if(length!=18) printf("Packet came without the standard length!\n"); |
||
188 | |||
189 | 959 | justin | /* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/
|
190 | /* Pull in data:*/
|
||
191 | 952 | justin | |
192 | short x,y,i;
|
||
193 | short ranges[5]; |
||
194 | |||
195 | x = ((short)packet[1] << 8) | (short)packet[0]; |
||
196 | y = ((short)packet[3] << 8) | (short)packet[2]; |
||
197 | |||
198 | for(i=0; i<5; i++){ |
||
199 | ranges[i] |
||
200 | = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i]; |
||
201 | } |
||
202 | |||
203 | float theta = *((float*)(packet + 4)); |
||
204 | |||
205 | 959 | justin | /*Write new data to buffers. (Lock down!)*/
|
206 | 952 | justin | pthread_rwlock_wrlock(&buffer_lock); |
207 | { |
||
208 | 959 | justin | /*Note that you write no matter what. */
|
209 | 952 | justin | tail_index = (tail_index + 1) % BUFFER_SIZE;
|
210 | |||
211 | 959 | justin | /*Write the robots number.*/
|
212 | 952 | justin | robot_buffer[tail_index] = source; |
213 | |||
214 | 959 | justin | /*Write in range data.*/
|
215 | 952 | justin | for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i]; |
216 | |||
217 | coordinate_buffer[tail_index][0] = x;
|
||
218 | coordinate_buffer[tail_index][1] = y;
|
||
219 | |||
220 | orientation_buffer[tail_index] = theta; |
||
221 | |||
222 | 959 | justin | /*If the oldest member didn't exist, buffer was empty.*/
|
223 | 952 | justin | if(head_index == BUFFER_EMPTY) head_index = 0; |
224 | |||
225 | 959 | justin | /*Otherwise check for overwritten data. */
|
226 | 952 | justin | else if(tail_index == head_index) |
227 | head_index = (head_index +1) % BUFFER_SIZE;
|
||
228 | |||
229 | } |
||
230 | pthread_rwlock_unlock(&buffer_lock); |
||
231 | |||
232 | 959 | justin | /*Increment for the transmit thread.*/
|
233 | 952 | justin | packets_received++; |
234 | } |
||
235 | } |