root / trunk / code / projects / mapping / matlab / receive.c @ 1134
History | View | Annotate | Download (5.58 KB)
1 | 966 | justin | /* This version of receive initializes
|
---|---|---|---|
2 | 1062 | justin | * the token ring and then waits for
|
3 | * the robots to send data.
|
||
4 | 966 | justin | */
|
5 | |||
6 | 952 | justin | #include <stdio.h> |
7 | #include <stdlib.h> |
||
8 | 959 | justin | #include <mex.h> |
9 | 952 | justin | #include <pthread.h> |
10 | #include <unistd.h> |
||
11 | 1093 | justin | #include <time.h> |
12 | 952 | justin | #include "../../libwireless/lib/wireless.h" |
13 | |||
14 | 1093 | justin | #define WL_COM_PORT "/dev/ttyUSB0" |
15 | 952 | justin | #define WL_CHANNEL 0xE |
16 | |||
17 | 960 | justin | #define MAP_REQUEST_GROUP 23 |
18 | 952 | justin | |
19 | 963 | justin | #define DATA_POINT 2 /* packet type for map data points w/ odometry data*/ |
20 | #define DATA_REQUEST 3 |
||
21 | 1032 | justin | #define DATA_SERVER_REPLY 4 /*Just to give the robot a target.*/ |
22 | #define DATA_SERVER_IDENTIFY 5 |
||
23 | 952 | justin | |
24 | #define BUFFER_SIZE 1000 |
||
25 | |||
26 | pthread_rwlock_t buffer_lock; |
||
27 | pthread_t receive_thread; |
||
28 | |||
29 | void packet_receive(char type, int source, unsigned char* packet, int length); |
||
30 | |||
31 | 963 | justin | PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL}; |
32 | 952 | justin | |
33 | 1093 | justin | struct robot_datapoint{
|
34 | time_t time; |
||
35 | short number;
|
||
36 | short range[5]; |
||
37 | short x;
|
||
38 | short y;
|
||
39 | float orientation;
|
||
40 | 952 | justin | |
41 | 1093 | justin | } robot_data[BUFFER_SIZE]; |
42 | |||
43 | 959 | justin | int tail_index; /*Points to the newest data in the buffers.*/ |
44 | int head_index; /*Points to the oldest data in the buffers.*/ |
||
45 | 952 | justin | |
46 | 1028 | justin | void* run_wireless(void* arg); |
47 | 966 | justin | void init_wireless(void); |
48 | 952 | justin | |
49 | 959 | justin | void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){ |
50 | |||
51 | if(nrhs == 1){ |
||
52 | 952 | justin | |
53 | 959 | justin | /*Initialization*/
|
54 | 1062 | justin | head_index = -1;
|
55 | tail_index = -1;
|
||
56 | 952 | justin | |
57 | pthread_rwlock_init(&buffer_lock, NULL);
|
||
58 | |||
59 | 1028 | justin | pthread_create(&receive_thread, NULL, run_wireless, NULL); |
60 | 959 | justin | return;
|
61 | 952 | justin | } |
62 | |||
63 | 959 | justin | /*Return stuff to matlab*/
|
64 | /*deliberately ignore nrhs. */
|
||
65 | |||
66 | 952 | justin | if(nlhs != 4){ |
67 | 959 | justin | mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n \
|
68 | where P = [X Y], IR = [IR1, IR2 ...],\n \
|
||
69 | 952 | justin | T = angle, and R = Robot number.");
|
70 | } |
||
71 | |||
72 | 1062 | justin | int target, index,i,j;
|
73 | 952 | justin | |
74 | 1062 | justin | if(tail_index == -1 || head_index == -1){ |
75 | double* point, *ranges, *theta, *robot;
|
||
76 | /*Appropriately return with nothing. (til I figure that out, return 0's)*/
|
||
77 | ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL); |
||
78 | ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL); |
||
79 | ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL); |
||
80 | ret[3] = mxCreateDoubleMatrix(1, 1, mxREAL); |
||
81 | 959 | justin | |
82 | 1062 | justin | point = mxGetPr(ret[0]);
|
83 | ranges = mxGetPr(ret[1]);
|
||
84 | theta = mxGetPr(ret[2]);
|
||
85 | robot = mxGetPr(ret[3]);
|
||
86 | 959 | justin | |
87 | 1062 | justin | point[0] = point[1] = 0.0; |
88 | 959 | justin | |
89 | 1062 | justin | ranges[0] = ranges[1] = ranges[2] = 0.0; |
90 | ranges[3] = ranges[4] = 0.0; |
||
91 | 959 | justin | |
92 | 1062 | justin | theta[0] = 0.0; |
93 | robot[0] = 0.0; |
||
94 | } |
||
95 | 1093 | justin | |
96 | 1062 | justin | else{ /*Copy the buffer data and set the buffer to empty.*/ |
97 | pthread_rwlock_rdlock(&buffer_lock); |
||
98 | { |
||
99 | 959 | justin | double** point, **ranges, *theta, *robot;
|
100 | 960 | justin | |
101 | 952 | justin | int num_buffer_elements = tail_index - head_index;
|
102 | if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE; |
||
103 | |||
104 | ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL); |
||
105 | ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL); |
||
106 | ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
||
107 | ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
||
108 | |||
109 | 959 | justin | point = (double**)mxGetPr(ret[0]); |
110 | ranges = (double**)mxGetPr(ret[1]); |
||
111 | 952 | justin | theta = mxGetPr(ret[2]);
|
112 | robot = mxGetPr(ret[3]);
|
||
113 | 959 | justin | |
114 | 1062 | justin | for(index = 0; index < num_buffer_elements; index++){ |
115 | 1093 | justin | int target = (head_index + index) % BUFFER_SIZE;
|
116 | point[i][0] = (double)robot_data[target].x; |
||
117 | point[i][1] = (double)robot_data[target].y; |
||
118 | 952 | justin | |
119 | for(j=0;j<5;j++) |
||
120 | 1093 | justin | ranges[i][j] = (double)robot_data[target].range[j];
|
121 | 952 | justin | |
122 | 1093 | justin | theta[i] = (double)(robot_data[target].orientation);
|
123 | robot[i] = (double)(robot_data[target].number);
|
||
124 | 952 | justin | } |
125 | 1093 | justin | |
126 | /*Relabel the indices 'empty' (-1)*/
|
||
127 | 1062 | justin | head_index = -1;
|
128 | tail_index = -1;
|
||
129 | 959 | justin | } |
130 | 1062 | justin | pthread_rwlock_unlock(&buffer_lock); |
131 | } |
||
132 | 952 | justin | } |
133 | |||
134 | 966 | justin | void init_wireless(void){ |
135 | /*General communication initialization. */
|
||
136 | wl_set_com_port(WL_COM_PORT); |
||
137 | wl_init(); |
||
138 | wl_set_channel(WL_CHANNEL); |
||
139 | wl_register_packet_group(&requestHandler); |
||
140 | } |
||
141 | |||
142 | 1028 | justin | |
143 | /*Call wl_do and occasionally send a broadcast packet declaring the
|
||
144 | * number of the server.
|
||
145 | 952 | justin | */
|
146 | 1028 | justin | void* run_wireless(void* arg){ |
147 | 1062 | justin | |
148 | 952 | justin | pthread_detach(pthread_self()); |
149 | |||
150 | 966 | justin | init_wireless(); |
151 | 1062 | justin | printf("Wireless initialized!\n");
|
152 | 952 | justin | |
153 | 1028 | justin | int a = 0; |
154 | 1062 | justin | /*Passively wait for packets to arrive.*/
|
155 | while(1) wl_do(); |
||
156 | 952 | justin | } |
157 | |||
158 | void packet_receive (char type, int source, unsigned char* packet, int length) { |
||
159 | 1093 | justin | printf("A packet came in!\n");
|
160 | 963 | justin | if(type==DATA_POINT){
|
161 | 952 | justin | |
162 | 1028 | justin | if(length!=18) |
163 | printf("Packet came without the standard length!\n");
|
||
164 | 952 | justin | |
165 | 959 | justin | /* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/
|
166 | /* Pull in data:*/
|
||
167 | 952 | justin | |
168 | short x,y,i;
|
||
169 | short ranges[5]; |
||
170 | |||
171 | 1093 | justin | /*Bytes 0-3*/
|
172 | 952 | justin | x = ((short)packet[1] << 8) | (short)packet[0]; |
173 | y = ((short)packet[3] << 8) | (short)packet[2]; |
||
174 | 1093 | justin | |
175 | /*Bytes 4-7*/
|
||
176 | float theta = *((float*)(packet + 4)); |
||
177 | |||
178 | /*Bytes 8-17*/
|
||
179 | 952 | justin | for(i=0; i<5; i++){ |
180 | ranges[i] |
||
181 | = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i]; |
||
182 | } |
||
183 | |||
184 | 959 | justin | /*Write new data to buffers. (Lock down!)*/
|
185 | 952 | justin | pthread_rwlock_wrlock(&buffer_lock); |
186 | { |
||
187 | 1062 | justin | /*Note that you write no matter what.
|
188 | * Also, if the buffer is empty, tail_index becomes 0*/
|
||
189 | 952 | justin | tail_index = (tail_index + 1) % BUFFER_SIZE;
|
190 | |||
191 | 959 | justin | /*Write the robots number.*/
|
192 | 1093 | justin | robot_data[tail_index].number = source; |
193 | 952 | justin | |
194 | 959 | justin | /*Write in range data.*/
|
195 | 1093 | justin | for(i=0; i<5; i++) robot_data[tail_index].range[i] = ranges[i]; |
196 | 952 | justin | |
197 | 1093 | justin | robot_data[tail_index].x = x; |
198 | robot_data[tail_index].y = y; |
||
199 | 952 | justin | |
200 | 1093 | justin | robot_data[tail_index].orientation = theta; |
201 | 952 | justin | |
202 | 959 | justin | /*If the oldest member didn't exist, buffer was empty.*/
|
203 | 1062 | justin | if(head_index == -1) head_index = 0; |
204 | 952 | justin | |
205 | 959 | justin | /*Otherwise check for overwritten data. */
|
206 | 952 | justin | else if(tail_index == head_index) |
207 | 1062 | justin | head_index = (head_index + 1) % BUFFER_SIZE;
|
208 | 952 | justin | |
209 | } |
||
210 | pthread_rwlock_unlock(&buffer_lock); |
||
211 | } |
||
212 | 1032 | justin | else if(type == DATA_SERVER_IDENTIFY){ |
213 | 1062 | justin | /*Sleep before sending an identity packet.*/
|
214 | 1093 | justin | printf("Announcing self as server\n");
|
215 | usleep(5000);
|
||
216 | 1032 | justin | wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0); |
217 | 1093 | justin | usleep(5000);
|
218 | 1028 | justin | } |
219 | 952 | justin | } |