Revision 1093
Cleaned up the code, someone help figure out the memory error!
trunk/code/projects/mapping/matlab/receive.c | ||
---|---|---|
8 | 8 |
#include <mex.h> |
9 | 9 |
#include <pthread.h> |
10 | 10 |
#include <unistd.h> |
11 |
#include <time.h> |
|
11 | 12 |
#include "../../libwireless/lib/wireless.h" |
12 |
#include "../../libwireless/lib/wl_token_ring.h" |
|
13 | 13 |
|
14 |
#define WL_COM_PORT "/dev/ttyUSB1"
|
|
14 |
#define WL_COM_PORT "/dev/ttyUSB0"
|
|
15 | 15 |
#define WL_CHANNEL 0xE |
16 | 16 |
|
17 | 17 |
#define MAP_REQUEST_GROUP 23 |
... | ... | |
30 | 30 |
|
31 | 31 |
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL}; |
32 | 32 |
|
33 |
/*Buffers*/ |
|
34 |
short robot_buffer[BUFFER_SIZE]; /*by #*/ |
|
35 |
short range_buffer[BUFFER_SIZE][5]; /*IR1, IR2 ... */ |
|
36 |
short coordinate_buffer[BUFFER_SIZE][2]; /*X Y*/ |
|
37 |
float orientation_buffer[BUFFER_SIZE]; |
|
33 |
struct robot_datapoint{ |
|
34 |
time_t time; |
|
35 |
short number; |
|
36 |
short range[5]; |
|
37 |
short x; |
|
38 |
short y; |
|
39 |
float orientation; |
|
38 | 40 |
|
41 |
} robot_data[BUFFER_SIZE]; |
|
42 |
|
|
39 | 43 |
int tail_index; /*Points to the newest data in the buffers.*/ |
40 | 44 |
int head_index; /*Points to the oldest data in the buffers.*/ |
41 | 45 |
|
... | ... | |
88 | 92 |
theta[0] = 0.0; |
89 | 93 |
robot[0] = 0.0; |
90 | 94 |
} |
95 |
|
|
91 | 96 |
else{ /*Copy the buffer data and set the buffer to empty.*/ |
92 | 97 |
pthread_rwlock_rdlock(&buffer_lock); |
93 | 98 |
{ |
... | ... | |
107 | 112 |
robot = mxGetPr(ret[3]); |
108 | 113 |
|
109 | 114 |
for(index = 0; index < num_buffer_elements; index++){ |
110 |
int target = (tail_index + index) % BUFFER_SIZE;
|
|
111 |
point[i][0] = (double)coordinate_buffer[target][0];
|
|
112 |
point[i][1] = (double)coordinate_buffer[target][1];
|
|
115 |
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;
|
|
113 | 118 |
|
114 | 119 |
for(j=0;j<5;j++) |
115 |
ranges[i][j] = (double)range_buffer[target][j];
|
|
120 |
ranges[i][j] = (double)robot_data[target].range[j];
|
|
116 | 121 |
|
117 |
theta[i] = (double)(orientation_buffer[target]);
|
|
118 |
robot[i] = (double)(robot_buffer[target]);
|
|
122 |
theta[i] = (double)(robot_data[target].orientation);
|
|
123 |
robot[i] = (double)(robot_data[target].number);
|
|
119 | 124 |
} |
125 |
|
|
126 |
/*Relabel the indices 'empty' (-1)*/ |
|
120 | 127 |
head_index = -1; |
121 | 128 |
tail_index = -1; |
122 | 129 |
} |
... | ... | |
130 | 137 |
wl_init(); |
131 | 138 |
wl_set_channel(WL_CHANNEL); |
132 | 139 |
wl_register_packet_group(&requestHandler); |
133 |
|
|
134 |
/*Token ring / sensor network initialization.*/ |
|
135 |
wl_token_ring_register(); |
|
136 | 140 |
} |
137 | 141 |
|
138 | 142 |
|
... | ... | |
152 | 156 |
} |
153 | 157 |
|
154 | 158 |
void packet_receive (char type, int source, unsigned char* packet, int length) { |
155 |
printf("A packet came in!\n");
|
|
159 |
printf("A packet came in!\n"); |
|
156 | 160 |
if(type==DATA_POINT){ |
157 | 161 |
|
158 | 162 |
if(length!=18) |
... | ... | |
164 | 168 |
short x,y,i; |
165 | 169 |
short ranges[5]; |
166 | 170 |
|
171 |
/*Bytes 0-3*/ |
|
167 | 172 |
x = ((short)packet[1] << 8) | (short)packet[0]; |
168 | 173 |
y = ((short)packet[3] << 8) | (short)packet[2]; |
169 |
|
|
174 |
|
|
175 |
/*Bytes 4-7*/ |
|
176 |
float theta = *((float*)(packet + 4)); |
|
177 |
|
|
178 |
/*Bytes 8-17*/ |
|
170 | 179 |
for(i=0; i<5; i++){ |
171 | 180 |
ranges[i] |
172 | 181 |
= ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i]; |
173 | 182 |
} |
174 | 183 |
|
175 |
float theta = *((float*)(packet + 4)); |
|
176 |
|
|
177 | 184 |
/*Write new data to buffers. (Lock down!)*/ |
178 | 185 |
pthread_rwlock_wrlock(&buffer_lock); |
179 | 186 |
{ |
... | ... | |
182 | 189 |
tail_index = (tail_index + 1) % BUFFER_SIZE; |
183 | 190 |
|
184 | 191 |
/*Write the robots number.*/ |
185 |
robot_buffer[tail_index] = source;
|
|
192 |
robot_data[tail_index].number = source;
|
|
186 | 193 |
|
187 | 194 |
/*Write in range data.*/ |
188 |
for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i];
|
|
195 |
for(i=0; i<5; i++) robot_data[tail_index].range[i] = ranges[i];
|
|
189 | 196 |
|
190 |
coordinate_buffer[tail_index][0] = x;
|
|
191 |
coordinate_buffer[tail_index][1] = y;
|
|
197 |
robot_data[tail_index].x = x;
|
|
198 |
robot_data[tail_index].y = y;
|
|
192 | 199 |
|
193 |
orientation_buffer[tail_index] = theta;
|
|
200 |
robot_data[tail_index].orientation = theta;
|
|
194 | 201 |
|
195 | 202 |
/*If the oldest member didn't exist, buffer was empty.*/ |
196 | 203 |
if(head_index == -1) head_index = 0; |
... | ... | |
204 | 211 |
} |
205 | 212 |
else if(type == DATA_SERVER_IDENTIFY){ |
206 | 213 |
/*Sleep before sending an identity packet.*/ |
207 |
usleep(50000); |
|
214 |
printf("Announcing self as server\n"); |
|
215 |
usleep(5000); |
|
208 | 216 |
wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0); |
209 |
printf("Announcing self as server\n");
|
|
217 |
usleep(5000);
|
|
210 | 218 |
} |
211 | 219 |
} |
212 | 220 |
|
Also available in: Unified diff