Revision 1062
Small modifications to receive.c, corrected an error
in the ring buffer. Some commenting and cleaning.
| trunk/code/projects/mapping/matlab/receive.c | ||
|---|---|---|
| 1 | 1 |
/* 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. |
|
| 2 |
* the token ring and then waits for |
|
| 3 |
* the robots to send data. |
|
| 5 | 4 |
*/ |
| 6 | 5 |
|
| 7 | 6 |
#include <stdio.h> |
| ... | ... | |
| 23 | 22 |
#define DATA_SERVER_IDENTIFY 5 |
| 24 | 23 |
|
| 25 | 24 |
#define BUFFER_SIZE 1000 |
| 26 |
#define BUFFER_EMPTY -1 |
|
| 27 | 25 |
|
| 28 | 26 |
pthread_rwlock_t buffer_lock; |
| 29 | 27 |
pthread_t receive_thread; |
| ... | ... | |
| 49 | 47 |
if(nrhs == 1){
|
| 50 | 48 |
|
| 51 | 49 |
/*Initialization*/ |
| 52 |
head_index = BUFFER_EMPTY;
|
|
| 53 |
tail_index = BUFFER_EMPTY;
|
|
| 50 |
head_index = -1;
|
|
| 51 |
tail_index = -1;
|
|
| 54 | 52 |
|
| 55 | 53 |
pthread_rwlock_init(&buffer_lock, NULL); |
| 56 | 54 |
|
| ... | ... | |
| 67 | 65 |
T = angle, and R = Robot number."); |
| 68 | 66 |
} |
| 69 | 67 |
|
| 70 |
int index,i,j; |
|
| 68 |
int target, index,i,j;
|
|
| 71 | 69 |
|
| 72 |
pthread_rwlock_rdlock(&buffer_lock); |
|
| 73 |
{
|
|
| 74 |
|
|
| 75 |
if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
|
|
| 76 |
double* point, *ranges, *theta, *robot; |
|
| 77 |
/*Appropriately return with nothing. (til I figure that out, return 0's)*/ |
|
| 78 |
ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL); |
|
| 79 |
ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL); |
|
| 80 |
ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL); |
|
| 81 |
ret[3] = mxCreateDoubleMatrix(1, 1, mxREAL); |
|
| 70 |
if(tail_index == -1 || head_index == -1){
|
|
| 71 |
double* point, *ranges, *theta, *robot; |
|
| 72 |
/*Appropriately return with nothing. (til I figure that out, return 0's)*/ |
|
| 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); |
|
| 82 | 77 |
|
| 83 |
point = mxGetPr(ret[0]);
|
|
| 84 |
ranges = mxGetPr(ret[1]);
|
|
| 85 |
theta = mxGetPr(ret[2]);
|
|
| 86 |
robot = mxGetPr(ret[3]);
|
|
| 78 |
point = mxGetPr(ret[0]); |
|
| 79 |
ranges = mxGetPr(ret[1]); |
|
| 80 |
theta = mxGetPr(ret[2]); |
|
| 81 |
robot = mxGetPr(ret[3]); |
|
| 87 | 82 |
|
| 88 |
point[0] = point[1] = 0.0;
|
|
| 83 |
point[0] = point[1] = 0.0; |
|
| 89 | 84 |
|
| 90 |
ranges[0] = ranges[1] = ranges[2] = 0.0;
|
|
| 91 |
ranges[3] = ranges[4] = 0.0;
|
|
| 85 |
ranges[0] = ranges[1] = ranges[2] = 0.0; |
|
| 86 |
ranges[3] = ranges[4] = 0.0; |
|
| 92 | 87 |
|
| 93 |
theta[0] = 0.0; |
|
| 94 |
robot[0] = 0.0; |
|
| 95 |
|
|
| 96 |
} |
|
| 97 |
else{ /*Copy the buffer data and set the buffer to empty.*/
|
|
| 88 |
theta[0] = 0.0; |
|
| 89 |
robot[0] = 0.0; |
|
| 90 |
} |
|
| 91 |
else{ /*Copy the buffer data and set the buffer to empty.*/
|
|
| 92 |
pthread_rwlock_rdlock(&buffer_lock); |
|
| 93 |
{
|
|
| 98 | 94 |
double** point, **ranges, *theta, *robot; |
| 99 | 95 |
|
| 100 | 96 |
int num_buffer_elements = tail_index - head_index; |
| ... | ... | |
| 110 | 106 |
theta = mxGetPr(ret[2]); |
| 111 | 107 |
robot = mxGetPr(ret[3]); |
| 112 | 108 |
|
| 113 |
for(index = head_index; index <= tail_index; index = (index + 1)%BUFFER_SIZE){
|
|
| 114 |
point[i][0] = (double)coordinate_buffer[index][0]; |
|
| 115 |
point[i][1] = (double)coordinate_buffer[index][1]; |
|
| 109 |
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]; |
|
| 116 | 113 |
|
| 117 | 114 |
for(j=0;j<5;j++) |
| 118 |
ranges[i][j] = (double)range_buffer[index][j];
|
|
| 115 |
ranges[i][j] = (double)range_buffer[target][j];
|
|
| 119 | 116 |
|
| 120 |
theta[i] = (double)(orientation_buffer[index]);
|
|
| 121 |
robot[i] = (double)(robot_buffer[index]);
|
|
| 117 |
theta[i] = (double)(orientation_buffer[target]);
|
|
| 118 |
robot[i] = (double)(robot_buffer[target]);
|
|
| 122 | 119 |
} |
| 123 |
head_index = BUFFER_EMPTY;
|
|
| 124 |
tail_index = BUFFER_EMPTY;
|
|
| 120 |
head_index = -1;
|
|
| 121 |
tail_index = -1;
|
|
| 125 | 122 |
} |
| 126 |
}
|
|
| 127 |
pthread_rwlock_unlock(&buffer_lock);
|
|
| 123 |
pthread_rwlock_unlock(&buffer_lock);
|
|
| 124 |
}
|
|
| 128 | 125 |
} |
| 129 | 126 |
|
| 130 | 127 |
void init_wireless(void){
|
| ... | ... | |
| 143 | 140 |
* number of the server. |
| 144 | 141 |
*/ |
| 145 | 142 |
void* run_wireless(void* arg){
|
| 143 |
|
|
| 146 | 144 |
pthread_detach(pthread_self()); |
| 147 | 145 |
|
| 148 | 146 |
init_wireless(); |
| 149 |
printf("Wireless initialized\n");
|
|
| 147 |
printf("Wireless initialized!\n");
|
|
| 150 | 148 |
|
| 151 | 149 |
int a = 0; |
| 152 |
while(1){
|
|
| 153 |
/*Passively wait for packets to arrive.*/ |
|
| 154 |
wl_do(); |
|
| 155 |
} |
|
| 150 |
/*Passively wait for packets to arrive.*/ |
|
| 151 |
while(1) wl_do(); |
|
| 156 | 152 |
} |
| 157 | 153 |
|
| 158 | 154 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| ... | ... | |
| 181 | 177 |
/*Write new data to buffers. (Lock down!)*/ |
| 182 | 178 |
pthread_rwlock_wrlock(&buffer_lock); |
| 183 | 179 |
{
|
| 184 |
/*Note that you write no matter what. */ |
|
| 180 |
/*Note that you write no matter what. |
|
| 181 |
* Also, if the buffer is empty, tail_index becomes 0*/ |
|
| 185 | 182 |
tail_index = (tail_index + 1) % BUFFER_SIZE; |
| 186 | 183 |
|
| 187 | 184 |
/*Write the robots number.*/ |
| ... | ... | |
| 196 | 193 |
orientation_buffer[tail_index] = theta; |
| 197 | 194 |
|
| 198 | 195 |
/*If the oldest member didn't exist, buffer was empty.*/ |
| 199 |
if(head_index == BUFFER_EMPTY) head_index = 0;
|
|
| 196 |
if(head_index == -1) head_index = 0;
|
|
| 200 | 197 |
|
| 201 | 198 |
/*Otherwise check for overwritten data. */ |
| 202 | 199 |
else if(tail_index == head_index) |
| 203 |
head_index = (head_index +1) % BUFFER_SIZE; |
|
| 200 |
head_index = (head_index + 1) % BUFFER_SIZE;
|
|
| 204 | 201 |
|
| 205 | 202 |
} |
| 206 | 203 |
pthread_rwlock_unlock(&buffer_lock); |
| 207 | 204 |
} |
| 208 | 205 |
else if(type == DATA_SERVER_IDENTIFY){
|
| 206 |
/*Sleep before sending an identity packet.*/ |
|
| 207 |
usleep(50000); |
|
| 209 | 208 |
wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0); |
| 210 | 209 |
printf("Announcing self as server\n");
|
| 211 | 210 |
} |
Also available in: Unified diff