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