Project

General

Profile

Revision 1345

Added by Rich Hong almost 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

wireless.c
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 *
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 *
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 *
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file wireless.c
28
 * @brief Wireless Library Implementation
29
 *
30
 * Implementation of the wireless library.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
1 35
#include "wireless.h"
2 36
#include "xbee.h"
3 37
#include <stdlib.h>
......
6 40
#include "wl_defs.h"
7 41

  
8 42
#ifndef ROBOT
9
#include <time.h>
10
#include <signal.h>
43
	#include <sys/time.h>
44
	#include <signal.h>
11 45
#else
12
#include <time.h>
13
#ifndef FIREFLY
14
#include <bom.h>
46
	#include <time.h>
47
	#include <bom.h>
15 48
#endif
16
#endif
17 49

  
18 50
/*Function Prototypes*/
19 51

  
20
void wl_do_timeout(void);
52
static void wl_do_timeout(void);
21 53

  
22 54
//Note: the actual frame sent has group as the first four bits and
23 55
//frame as the last four.
24
void wl_send_packet(char group, char type, char* data, int len,
25
					int dest, char options, char frame);
56
static int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame);
26 57

  
27 58
/*Data Members*/
28 59

  
29 60
//used to store incoming and outgoing packets
30
unsigned char wl_buf[128];
61
//TODO: does this need to be 128?  can it be smaller to save memory?
62
//TODO: this shouldn't be hardcoded as 128.  it should be a define.
63
static unsigned char wl_buf[128];
31 64
//1 if we have timed out since we last checked, 0 otherwise.
32
int wl_timeout = 0;
65
static int wl_timeout = 0;
33 66

  
34
PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
67
static PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
35 68

  
36 69
#ifndef ROBOT
37
timer_t wl_timeout_timer;
38 70

  
39 71
//called when we time out, or receive interrupt
40
void sig_handler(int signo)
72
static void sig_handler(int signo)
41 73
{
42 74
	switch (signo)
43 75
	{
......
51 83
	}
52 84
	return;
53 85
}
86
#else
87

  
88
//called when the timer ticks
89
static void timer_handler(void)
90
{
91
	wl_timeout = 1;
92
}
93

  
54 94
#endif
55 95

  
56 96
/**
57 97
 * Initializes the wireless library. Must be called before any
58 98
 * other function.
99
 *
100
 * @param wl_port File descriptor for wireless port, or NULL for default.
59 101
 **/
60
void wl_init()
102
int wl_init()
61 103
{
62 104
	int i;
105
  //TODO: using memset here instead of this loop, *might* be less instructions and *might* reduce code size but not sure
63 106
	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++)
64 107
		wl_packet_groups[i] = NULL;
65 108

  
66
	xbee_lib_init();
67
	
109
	if (xbee_lib_init() == -1) {
110
		return -1;
111
	}
112

  
68 113
	//begin timeout timer
69
	#ifdef ROBOT
70
	#ifdef FIREFLY
71
	rtc_init(PRESCALE_DIV_128, 32, &wl_do_timeout);
114
#ifdef ROBOT
115
#ifdef FIREFLY
116
	rtc_init(PRESCALE_DIV_256, 32, &timer_handler);
117
#else
118
	//TODO: FIX THIS
119
	#ifdef BAYBOARD
120
	rtc_init(10 * HALF_SECOND, &timer_handler);
72 121
	#else
73
	rtc_init(HALF_SECOND, &wl_do_timeout); 
122
	rtc_init(HALF_SECOND, &timer_handler);
74 123
	#endif
75
	#else
76
	//create a timer to trigger every half second
77
	struct sigevent evp;
78
	evp.sigev_signo = SIGALRM;
79
	evp.sigev_notify = SIGEV_SIGNAL;
80
	if (timer_create(CLOCK_REALTIME, &evp, &wl_timeout_timer) == -1)
81
	{ 
82
		WL_DEBUG_PRINT("Error creating a timer.\r\n"); 
83
		exit(1); 
124
#endif
125
#else
126

  
127
	//create our timer
128
	struct itimerval timer_val;
129
	struct timeval interval;
130
	interval.tv_sec = 0;
131
	interval.tv_usec = 500000;
132
	struct timeval first_time;
133
	first_time.tv_sec = 0;
134
	first_time.tv_usec = 500000;
135
	timer_val.it_interval = interval;
136
	timer_val.it_value = first_time;
137
	if(setitimer(ITIMER_REAL,&timer_val,NULL)==-1)
138
	{
139
		WL_DEBUG_PRINT("Error creating a timer.\r\n");
140
		perror("Failure's cause");
141
		exit(1);
84 142
	}
143

  
144
	//create signal handler
85 145
	struct sigaction wl_sig_act;
86
	wl_sig_act.sa_handler = (void *)sig_handler;
146
	wl_sig_act.sa_handler = sig_handler;
87 147
	wl_sig_act.sa_flags = 0;
88 148
	sigemptyset(&wl_sig_act.sa_mask);
89 149
	sigaction(SIGALRM, &wl_sig_act, 0);
90 150
	sigaction(SIGINT, &wl_sig_act, 0);
91
	struct itimerspec wl_timeout_time;
92
	wl_timeout_time.it_interval.tv_sec = 0;
93
	wl_timeout_time.it_interval.tv_nsec = 500000000;
94
	wl_timeout_time.it_value.tv_sec = 0;
95
	wl_timeout_time.it_value.tv_nsec = 500000000;
96
	timer_settime(wl_timeout_timer, 0,
97
                        &wl_timeout_time, NULL);
98
	#endif
151
#endif
152

  
153
	return 0;
99 154
}
100 155

  
101 156
/**
......
103 158
 **/
104 159
void wl_terminate()
105 160
{
106
	#ifndef ROBOT
107
	timer_delete(wl_timeout_timer);
108
	#endif
109
	
110 161
	int i;
111
	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++)
162
	for (i = 0; i < WL_MAX_PACKET_GROUPS; i++) {
112 163
		if (wl_packet_groups[i] != NULL &&
113
			wl_packet_groups[i]->unregister != NULL)
164
			wl_packet_groups[i]->unregister != NULL) {
114 165
			wl_packet_groups[i]->unregister();
115
	
166
		}
167
	}
168

  
116 169
	xbee_terminate();
117 170
}
118 171

  
......
123 176
 *
124 177
 * @see wl_get_pan
125 178
 **/
126
void wl_set_pan(int pan)
179
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
180
// it reduces code size or not should be done to be sure.
181
int wl_set_pan(int pan)
127 182
{
128
	xbee_set_pan_id(pan);
183
	return xbee_set_pan_id(pan);
129 184
}
130 185

  
131 186
/**
......
135 190
 *
136 191
 * @see wl_set_pan
137 192
 **/
193
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
194
// it reduces code size or not should be done to be sure.
138 195
int wl_get_pan(void)
139 196
{
140 197
	return xbee_get_pan_id();
......
147 204
 *
148 205
 * @see wl_get_channel
149 206
 **/
150
void wl_set_channel(int channel)
207
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
208
// it reduces code size or not should be done to be sure.
209
int wl_set_channel(int channel)
151 210
{
152
	xbee_set_channel(channel);
211
	return xbee_set_channel(channel);
153 212
}
154 213

  
155 214
/**
......
159 218
 *
160 219
 * @see wl_set_channel
161 220
 **/
221
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
222
// it reduces code size or not should be done to be sure.
162 223
int wl_get_channel(void)
163 224
{
164 225
	return xbee_get_channel();
......
169 230
 *
170 231
 * @return the 16-bit address of the XBee module.
171 232
 **/
172
unsigned int wl_get_xbee_id()
233
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
234
// it reduces code size or not should be done to be sure.
235
int wl_get_xbee_id()
173 236
{
174 237
	return xbee_get_address();
175 238
}
......
184 247
 * @param dest the 16-bit address of the XBee to send the packet to
185 248
 * @param frame the frame number to see with a TX_STATUS response
186 249
 **/
187
void wl_send_robot_to_robot_global_packet(char group, char type,
188
		char* data, int len, int dest, char frame)
250
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
251
// it reduces code size or not should be done to be sure.
252
int wl_send_robot_to_robot_global_packet(char group, char type, char* data, int len, int dest, char frame)
189 253
{
190
	wl_send_packet(group, type, data, len, dest,
191
			XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
254
	return wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
192 255
}
193 256

  
194 257
/**
......
201 264
 * @param dest the 16-bit address of the XBee to send the packet to
202 265
 * @param frame the frame number to see with a TX_STATUS response
203 266
 **/
204
void wl_send_robot_to_robot_packet(char group, char type,
205
		char* data, int len, int dest, char frame)
267
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
268
// it reduces code size or not should be done to be sure.
269
int wl_send_robot_to_robot_packet(char group, char type, char* data, int len, int dest, char frame)
206 270
{
207
	wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_NONE,
208
			frame);
271
	return wl_send_packet(group, type, data, len, dest, XBEE_OPTIONS_NONE, frame);
209 272
}
210 273

  
211 274
/**
......
217 280
 * @param len the packet length in bytes
218 281
 * @param frame the frame number to see with a TX_STATUS response
219 282
 **/
220
void wl_send_global_packet(char group, char type,
221
		char* data, int len, char frame)
283
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
284
// it reduces code size or not should be done to be sure.
285
int wl_send_global_packet(char group, char type, char* data, int len, char frame)
222 286
{
223
	wl_send_packet(group, type, data, len, XBEE_BROADCAST,
224
			XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
287
	return wl_send_packet(group, type, data, len, XBEE_BROADCAST, XBEE_OPTIONS_BROADCAST_ALL_PANS, frame);
225 288
}
226 289

  
227 290
/**
......
233 296
 * @param len the packet length in bytes
234 297
 * @param frame the frame number to see with a TX_STATUS response
235 298
 **/
236
void wl_send_pan_packet(char group, char type,
237
		char* data, int len, char frame)
299
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
300
// it reduces code size or not should be done to be sure.
301
void wl_send_pan_packet(char group, char type, char* data, int len, char frame)
238 302
{
239
	wl_send_packet(group, type, data, len, XBEE_BROADCAST, 
303
	wl_send_packet(group, type, data, len, XBEE_BROADCAST,
240 304
			XBEE_OPTIONS_NONE, frame);
241 305
}
242 306

  
......
251 315
 * @param options the options for sending the packet
252 316
 * @param frame the frame number to see with a TX_STATUS response
253 317
 **/
254
void wl_send_packet(char group, char type, char* data, int len,
255
					int dest, char options, char frame)
318
int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame)
256 319
{
320
  //TODO: does this need to be 128?  can it be smaller to save memory?
321
  //TODO: this shouldn't be hardcoded as 128.  it should be a define.
257 322
	char buf[128];
258 323
	int i;
259 324
	if (frame != 0)
260 325
		frame = (frame & 0x0F) | ((group & 0x0F) << 4);
326

  
261 327
	buf[0] = group;
262 328
	buf[1] = type;
263 329
	for (i = 0; i < len; i++)
264 330
		buf[2 + i] = data[i];
265
	xbee_send_packet(buf, len + 2, dest, options, frame);
331

  
332
	return xbee_send_packet(buf, len + 2, dest, options, frame);
266 333
}
267 334

  
268 335
/**
......
289 356

  
290 357
/**
291 358
 * Unregister a packet group from the wireless library.
292
 * 
359
 *
293 360
 * @param h the packet group to remove
294 361
 **/
295 362
void wl_unregister_packet_group(PacketGroupHandler* h)
......
327 394
		wl_do_timeout();
328 395
		wl_timeout = 0;
329 396
	}
330
	
397

  
331 398
	int len = xbee_get_packet(wl_buf);
332 399
	if (len < 0)//no packet received
333 400
		return;
334
	
401

  
335 402
	if (wl_buf[0] == XBEE_TX_STATUS)
336 403
	{
337 404
		if (len != 3)
......
339 406
			WL_DEBUG_PRINT("Transmit Status packet should be of length 3.\r\n");
340 407
			return;
341 408
		}
342
		
409

  
343 410
		//the first four bits are the packet group
344 411
		//this only works with under 16 groups
345 412
		int group = (int)(wl_buf[1] >> 4);
346 413
		int success = 0;
347 414
		if (wl_buf[2] == 0)
415
		{
348 416
			success = 1;
417
		}
349 418
		else
350 419
		{
351 420
			WL_DEBUG_PRINT("No response received.\r\n");
352 421
			if (wl_buf[2] == 2)
422
			{
353 423
				WL_DEBUG_PRINT("CCA Failure\r\n");
424
			}
354 425
			if (wl_buf[2] == 3)
426
			{
355 427
				WL_DEBUG_PRINT("Purged\r\n");
428
			}
356 429
		}
357
		
358
		if (wl_packet_groups[group] != NULL &&
359
					wl_packet_groups[group]->handle_response != NULL)
360
			wl_packet_groups[group]->handle_response(
361
					(int)wl_buf[1] & 0x0F, success);
362
		return;
430

  
431
		if (wl_packet_groups[group] != NULL && wl_packet_groups[group]->handle_response != NULL)
432
			wl_packet_groups[group]->handle_response((int)wl_buf[1] & 0x0F, success);
363 433
	}
364
	
365
	if (wl_buf[0] == XBEE_RX)
434
	else if (wl_buf[0] == XBEE_RX)
366 435
	{
436
    //TODO: what does this 7 represent?  It shouldn't be hardcoded.  It should be set as a define
367 437
		if (len < 7)
368 438
		{
369 439
			WL_DEBUG_PRINT("Packet is too small.\r\n");
370 440
			return;
371 441
		}
372
		
442

  
373 443
		int source = ((int)wl_buf[1] << 8) + ((int)wl_buf[2]);
374
		
444

  
375 445
		/*
376 446
		//unused for now
377 447
		int signalStrength = wl_buf[3];
378 448
		//1 for Address broadcast, 2 for PAN broadcast
379 449
		int options = wl_buf[4];
380 450
		*/
381
		
451

  
452
    //TODO: these indices, etc should be defined, not hardcoded
382 453
		int group = wl_buf[5];
383 454
		int type = wl_buf[6];
384 455
		int packetLen = len - 7;
385
		
386
		if (wl_packet_groups[group] != NULL
387
				&& wl_packet_groups[group]->handle_receive != NULL)
388
			wl_packet_groups[group]->handle_receive(type, source, 
389
				wl_buf + 7, packetLen);
390
		return;
456

  
457
		if (wl_packet_groups[group] != NULL && wl_packet_groups[group]->handle_receive != NULL) {
458
			wl_packet_groups[group]->handle_receive(type, source, wl_buf + 7, packetLen);
459
		}
391 460
	}
392
	
393
	WL_DEBUG_PRINT("Unexpected packet received from XBee.\r\n");
394
	return;
461
	else
462
	{
463
		WL_DEBUG_PRINT("Unexpected packet received from XBee.\r\n");
464
		printf("0x%2X\n", wl_buf[0]);
465
		printf("%c%c%d\n", wl_buf[2], wl_buf[3], wl_buf[4]);
466
	}
395 467
}
396 468

  
469

  
470
#ifndef ROBOT
471
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
472
// it reduces code size or not should be done to be sure.
473
void wl_set_com_port(char* port)
474
{
475
	xbee_set_com_port(port);
476
}
477
#endif
478

  

Also available in: Unified diff