Project

General

Profile

Revision 1093

Cleaned up the code, someone help figure out the memory error!

View differences:

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