Revision 1361
Made some small changes reading through the code, no mex file compiler at home so I might have broken it.. but probably not.
| receive.c | ||
|---|---|---|
| 31 | 31 |
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
|
| 32 | 32 |
|
| 33 | 33 |
struct robot_datapoint{
|
| 34 |
time_t time; |
|
| 34 |
time_t time; //TODO write time too.
|
|
| 35 | 35 |
short number; |
| 36 | 36 |
short range[5]; |
| 37 | 37 |
short x; |
| ... | ... | |
| 98 | 98 |
{
|
| 99 | 99 |
double** point, **ranges, *theta, *robot; |
| 100 | 100 |
|
| 101 |
int num_buffer_elements = tail_index - head_index;
|
|
| 102 |
if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;
|
|
| 101 |
int num_buffer_elements = tail_index >= head_index ?
|
|
| 102 |
tail_index - head_index : tail_index - head_index + BUFFER_SIZE;
|
|
| 103 | 103 |
|
| 104 | 104 |
ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL); |
| 105 | 105 |
ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL); |
| ... | ... | |
| 112 | 112 |
robot = mxGetPr(ret[3]); |
| 113 | 113 |
|
| 114 | 114 |
for(index = 0; index < num_buffer_elements; index++){
|
| 115 |
int target = (head_index + index) % BUFFER_SIZE;
|
|
| 115 |
int target = (head_index + index) % BUFFER_SIZE; |
|
| 116 | 116 |
point[i][0] = (double)robot_data[target].x; |
| 117 | 117 |
point[i][1] = (double)robot_data[target].y; |
| 118 | 118 |
|
| ... | ... | |
| 167 | 167 |
|
| 168 | 168 |
short x,y,i; |
| 169 | 169 |
short ranges[5]; |
| 170 |
struct robot_datapoint data; |
|
| 170 | 171 |
|
| 171 | 172 |
/*Bytes 0-3*/ |
| 172 |
x = ((short)packet[1] << 8) | (short)packet[0]; |
|
| 173 |
y = ((short)packet[3] << 8) | (short)packet[2]; |
|
| 173 |
data.x = ((short)packet[1] << 8) | (short)packet[0]; |
|
| 174 |
data.y = ((short)packet[3] << 8) | (short)packet[2]; |
|
| 175 |
|
|
| 174 | 176 |
|
| 175 | 177 |
/*Bytes 4-7*/ |
| 176 |
float theta = *((float*)(packet + 4));
|
|
| 178 |
data.orientation = *((float*)(packet + 4));
|
|
| 177 | 179 |
|
| 178 | 180 |
/*Bytes 8-17*/ |
| 179 | 181 |
for(i=0; i<5; i++){
|
| 180 |
ranges[i]
|
|
| 182 |
data.range[i]
|
|
| 181 | 183 |
= ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i]; |
| 182 | 184 |
} |
| 183 |
|
|
| 185 |
|
|
| 184 | 186 |
/*Write new data to buffers. (Lock down!)*/ |
| 185 | 187 |
pthread_rwlock_wrlock(&buffer_lock); |
| 186 | 188 |
{
|
| 187 | 189 |
/*Note that you write no matter what. |
| 188 | 190 |
* Also, if the buffer is empty, tail_index becomes 0*/ |
| 189 | 191 |
tail_index = (tail_index + 1) % BUFFER_SIZE; |
| 190 |
|
|
| 192 |
|
|
| 191 | 193 |
/*Write the robots number.*/ |
| 192 | 194 |
robot_data[tail_index].number = source; |
| 193 | 195 |
|
| ... | ... | |
| 201 | 203 |
|
| 202 | 204 |
/*If the oldest member didn't exist, buffer was empty.*/ |
| 203 | 205 |
if(head_index == -1) head_index = 0; |
| 204 |
|
|
| 205 |
/*Otherwise check for overwritten data. */
|
|
| 206 |
|
|
| 207 |
/*Otherwise check for overwritten data.*/ |
|
| 206 | 208 |
else if(tail_index == head_index) |
| 207 | 209 |
head_index = (head_index + 1) % BUFFER_SIZE; |
| 208 |
|
|
| 209 | 210 |
} |
| 210 | 211 |
pthread_rwlock_unlock(&buffer_lock); |
| 211 | 212 |
} |
| 212 | 213 |
else if(type == DATA_SERVER_IDENTIFY){
|
| 213 | 214 |
/*Sleep before sending an identity packet.*/ |
| 214 | 215 |
printf("Announcing self as server\n");
|
| 215 |
usleep(5000); |
|
| 216 | 216 |
wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0); |
| 217 |
usleep(5000); |
|
| 218 | 217 |
} |
| 219 | 218 |
} |
| 220 | 219 |
|
Also available in: Unified diff