Revision 959
Changed receive to work when there's no data,
haven't tested for when there is,
test.c works as expected,
Makefile modified to work on receive.c on a linux machine with matlab.
testing robots pending - not hopeful - see code.
trunk/code/projects/mapping/matlab/receive.c | ||
---|---|---|
1 | 1 |
#include <stdio.h> |
2 | 2 |
#include <stdlib.h> |
3 |
//#include <mex.h>
|
|
3 |
#include <mex.h> |
|
4 | 4 |
#include <pthread.h> |
5 | 5 |
#include <unistd.h> |
6 | 6 |
#include "../../libwireless/lib/wireless.h" |
... | ... | |
12 | 12 |
#define MAP_RECEIVE_GROUP 1 |
13 | 13 |
#define MAP_REQUEST_GROUP 2 |
14 | 14 |
|
15 |
#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
|
|
15 |
#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 | 17 |
|
18 | 18 |
#define BUFFER_SIZE 1000 |
19 | 19 |
#define BUFFER_EMPTY -1 |
... | ... | |
21 | 21 |
pthread_rwlock_t buffer_lock; |
22 | 22 |
pthread_t receive_thread; |
23 | 23 |
|
24 |
int packets_received; //Used to block overlapping packet requests.
|
|
24 |
int packets_received; /*Used to block overlapping packet requests.*/
|
|
25 | 25 |
|
26 | 26 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
27 | 27 |
|
28 | 28 |
PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL}; |
29 | 29 |
PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL}; |
30 | 30 |
|
31 |
//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
|
|
31 |
/*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 | 35 |
float orientation_buffer[BUFFER_SIZE]; |
36 | 36 |
|
37 |
int tail_index; //Points to the newest data in the buffers.
|
|
38 |
int head_index; //Points to the oldest data in the buffers.
|
|
37 |
int tail_index; /*Points to the newest data in the buffers.*/
|
|
38 |
int head_index; /*Points to the oldest data in the buffers.*/
|
|
39 | 39 |
|
40 |
int receive_initialized = 0; |
|
41 |
|
|
42 | 40 |
void* buffer_data(void* arg); |
43 | 41 |
|
44 |
//void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
|
|
45 |
int main(){ |
|
46 |
if(!receive_initialized){
|
|
42 |
void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){ |
|
43 |
|
|
44 |
if(nrhs == 1){
|
|
47 | 45 |
|
48 |
//Initialization
|
|
46 |
/*Initialization*/
|
|
49 | 47 |
head_index = BUFFER_EMPTY; |
50 | 48 |
tail_index = BUFFER_EMPTY; |
51 | 49 |
|
52 | 50 |
pthread_rwlock_init(&buffer_lock, NULL); |
53 | 51 |
|
54 | 52 |
pthread_create(&receive_thread, NULL, buffer_data, NULL); |
55 |
receive_initialized = 1;
|
|
53 |
return;
|
|
56 | 54 |
} |
57 | 55 |
|
58 |
while(1){} |
|
59 |
|
|
60 |
/*Return stuff to matlab |
|
61 |
//deliberately ignore nrhs. |
|
56 |
/*Return stuff to matlab*/ |
|
57 |
/*deliberately ignore nrhs. */ |
|
58 |
|
|
62 | 59 |
if(nlhs != 4){ |
63 |
mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n |
|
64 |
where P = [X Y], IR = [IR1, IR2 ...],\n |
|
60 |
mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n \
|
|
61 |
where P = [X Y], IR = [IR1, IR2 ...],\n \
|
|
65 | 62 |
T = angle, and R = Robot number."); |
66 | 63 |
} |
67 | 64 |
|
68 |
double* point, ranges, theta; |
|
69 | 65 |
int index,i,j; |
70 | 66 |
|
71 | 67 |
pthread_rwlock_rdlock(&buffer_lock); |
72 | 68 |
{ |
73 | 69 |
|
74 | 70 |
if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){ |
75 |
//Appropriately return with nothing. |
|
71 |
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; |
|
76 | 92 |
} |
77 | 93 |
else{ |
78 |
//TODO Check for the 'already empty' case. |
|
94 |
double** point, **ranges, *theta, *robot; |
|
95 |
/*TODO Check for the 'already empty' case.*/ |
|
79 | 96 |
|
80 | 97 |
int num_buffer_elements = tail_index - head_index; |
81 | 98 |
if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE; |
... | ... | |
85 | 102 |
ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
86 | 103 |
ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL); |
87 | 104 |
|
88 |
point = mxGetPr(ret[0]); |
|
89 |
ranges = mxGetPr(ret[1]); |
|
105 |
/*This may not work on some computers.*/ |
|
106 |
point = (double**)mxGetPr(ret[0]); |
|
107 |
ranges = (double**)mxGetPr(ret[1]); |
|
90 | 108 |
theta = mxGetPr(ret[2]); |
91 | 109 |
robot = mxGetPr(ret[3]); |
92 |
|
|
110 |
|
|
93 | 111 |
for(index = head_index; index <= tail_index; index = (index + 1)%1024){ |
94 | 112 |
point[i][0] = (double)coordinate_buffer[index][0]; |
95 | 113 |
point[i][1] = (double)coordinate_buffer[index][1]; |
... | ... | |
102 | 120 |
} |
103 | 121 |
head_index = BUFFER_EMPTY; |
104 | 122 |
tail_index = BUFFER_EMPTY; |
123 |
} |
|
105 | 124 |
} |
106 | 125 |
pthread_rwlock_unlock(&buffer_lock); |
107 |
*/ |
|
108 |
return -1; |
|
109 | 126 |
} |
110 | 127 |
|
111 | 128 |
/* |
... | ... | |
117 | 134 |
|
118 | 135 |
pthread_detach(pthread_self()); |
119 | 136 |
|
120 |
//General communication initialization.
|
|
137 |
/*General communication initialization. */
|
|
121 | 138 |
wl_set_com_port(WL_COM_PORT); |
122 | 139 |
wl_init(); |
123 | 140 |
wl_set_channel(WL_CHANNEL); |
124 | 141 |
wl_register_packet_group(&receiveHandler); |
125 | 142 |
|
126 |
//Token ring / sensor network initialization.
|
|
143 |
/*Token ring / sensor network initialization.*/
|
|
127 | 144 |
wl_token_ring_register(); |
128 | 145 |
|
129 |
//Continuously iterate over all the robots - buffering data.
|
|
146 |
/*Continuously iterate over all the robots - buffering data.*/
|
|
130 | 147 |
while(1){ |
131 | 148 |
|
132 | 149 |
wl_token_iterator_begin(); |
133 | 150 |
|
134 | 151 |
num_robots = wl_token_get_num_robots(); |
135 | 152 |
|
136 |
//Initialize the 'packets received' number.
|
|
153 |
/*Initialize the 'packets received' number. */
|
|
137 | 154 |
packets_received = 0; |
138 | 155 |
|
139 |
//Send a data request to all robots.
|
|
156 |
/*Send a data request to all robots.*/
|
|
140 | 157 |
while(wl_token_iterator_has_next()){ |
141 | 158 |
robot_num = wl_token_iterator_next(); |
142 | 159 |
|
... | ... | |
146 | 163 |
|
147 | 164 |
} |
148 | 165 |
|
149 |
//Wait for all packets to be received before sending more requests.
|
|
166 |
/*Wait for all packets to be received before sending more requests.*/
|
|
150 | 167 |
while(packets_received < num_robots |
151 | 168 |
&& packets_received < wl_token_get_num_robots()){ |
152 | 169 |
|
153 |
usleep(10000); //10ms
|
|
170 |
usleep(10000); /*10ms*/
|
|
154 | 171 |
} |
155 | 172 |
|
156 | 173 |
usleep(10000); |
... | ... | |
163 | 180 |
|
164 | 181 |
if(length!=18) printf("Packet came without the standard length!\n"); |
165 | 182 |
|
166 |
// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5
|
|
167 |
// Pull in data:
|
|
183 |
/* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/
|
|
184 |
/* Pull in data:*/
|
|
168 | 185 |
|
169 | 186 |
short x,y,i; |
170 | 187 |
short ranges[5]; |
... | ... | |
179 | 196 |
|
180 | 197 |
float theta = *((float*)(packet + 4)); |
181 | 198 |
|
182 |
//Write new data to buffers. (Lock down!)
|
|
199 |
/*Write new data to buffers. (Lock down!)*/
|
|
183 | 200 |
pthread_rwlock_wrlock(&buffer_lock); |
184 | 201 |
{ |
185 |
//Note that you write no matter what.
|
|
202 |
/*Note that you write no matter what. */
|
|
186 | 203 |
tail_index = (tail_index + 1) % BUFFER_SIZE; |
187 | 204 |
|
188 |
//Write the robots number.
|
|
205 |
/*Write the robots number.*/
|
|
189 | 206 |
robot_buffer[tail_index] = source; |
190 | 207 |
|
191 |
//Write in range data.
|
|
208 |
/*Write in range data.*/
|
|
192 | 209 |
for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i]; |
193 | 210 |
|
194 | 211 |
coordinate_buffer[tail_index][0] = x; |
... | ... | |
196 | 213 |
|
197 | 214 |
orientation_buffer[tail_index] = theta; |
198 | 215 |
|
199 |
//If the oldest member didn't exist, buffer was empty.
|
|
216 |
/*If the oldest member didn't exist, buffer was empty.*/
|
|
200 | 217 |
if(head_index == BUFFER_EMPTY) head_index = 0; |
201 | 218 |
|
202 |
//Otherwise check for overwritten data.
|
|
219 |
/*Otherwise check for overwritten data. */
|
|
203 | 220 |
else if(tail_index == head_index) |
204 | 221 |
head_index = (head_index +1) % BUFFER_SIZE; |
205 | 222 |
|
206 | 223 |
} |
207 | 224 |
pthread_rwlock_unlock(&buffer_lock); |
208 | 225 |
|
209 |
//Increment for the transmit thread.
|
|
226 |
/*Increment for the transmit thread.*/
|
|
210 | 227 |
packets_received++; |
211 | 228 |
} |
212 | 229 |
} |
trunk/code/projects/mapping/matlab/testMex/test.c | ||
---|---|---|
4 | 4 |
pthread_t counter; |
5 | 5 |
|
6 | 6 |
void* count(void* arg); |
7 |
int first_time = 0, global = 0;
|
|
7 |
int first_time = 1, global = 0;
|
|
8 | 8 |
|
9 | 9 |
void mexFunction(int nlhs, |
10 | 10 |
mxArray* ret[], int nrhs, const mxArray* args[]) |
11 | 11 |
{ |
12 |
//deliberately ignore nrhs.
|
|
12 |
/*deliberately ignore nrhs. */
|
|
13 | 13 |
if(nlhs != 1){ |
14 | 14 |
mexErrMsgTxt("Error - only returns one number.\n"); |
15 | 15 |
} |
16 | 16 |
else{ |
17 | 17 |
|
18 | 18 |
if(first_time){ |
19 |
//Deploy a thread to count.
|
|
19 |
/*Deploy a thread to count.*/
|
|
20 | 20 |
pthread_create(&counter, NULL, count, NULL); |
21 |
first_time = 0; |
|
21 | 22 |
} |
22 | 23 |
|
23 | 24 |
ret[0] = mxCreateDoubleMatrix(1, 1, mxREAL); |
... | ... | |
27 | 28 |
} |
28 | 29 |
|
29 | 30 |
void* count(void* arg){ |
30 |
global++; |
|
31 |
usleep(10000); |
|
31 |
while(1){ |
|
32 |
global++; |
|
33 |
usleep(10000); |
|
34 |
} |
|
32 | 35 |
} |
trunk/code/projects/mapping/matlab/Makefile | ||
---|---|---|
1 |
CC= gcc
|
|
2 |
CFLAGS= -Wall -g
|
|
1 |
CC= mex
|
|
2 |
CFLAGS= |
|
3 | 3 |
LIBS= -lpthread -lwireless |
4 | 4 |
|
5 |
all: test
|
|
5 |
all: receive
|
|
6 | 6 |
|
7 | 7 |
test: *.c ../../libwireless/lib/*.c |
8 |
$(CC) $(CFLAGS) *.c -L../../../lib/bin -o test -DWL_DEBUG $(LIBS)
|
|
8 |
$(CC) $(CFLAGS) *.c -L../../../lib/bin -DWL_DEBUG $(LIBS) |
|
9 | 9 |
|
10 |
receive: |
|
11 |
mex -g receive.c -L../../libwireless/lib -lwireless |
|
12 |
|
|
10 | 13 |
clean: |
11 |
rm *.o test ../lib/*.o
|
|
14 |
rm *.o |
|
12 | 15 |
|
Also available in: Unified diff