X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=main.c;h=e9454c405bf6902bff2a1ae3dd67f64d9f45119f;hp=d5c3f5a34bf1df30f013a09b653287c3f1e3848e;hb=9a9b64112aee5ab26398b46cb13b7e49c292a355;hpb=8a557d146e15bf4bff2cf9214220c8f8e751658c

diff --git a/main.c b/main.c
index d5c3f5a..e9454c4 100644
--- a/main.c
+++ b/main.c
@@ -57,9 +57,12 @@
 #include "cmdline.h"
 #include "callout.h"
 #include "rc_proto.h"
+#include "spi_servo.h"
 #include "main.h"
 
 struct xbeeboard xbeeboard;
+volatile uint16_t global_ms;
+struct callout_manager cm;
 
 #define TIMEOUT_MS 1000
 
@@ -85,7 +88,7 @@ static void hexdump(const char *title, const void *buf, unsigned int len)
 #define LINE_LEN 80
 	char line[LINE_LEN];	/* space needed 8+16*3+3+16 == 75 */
 
-	printf_P(PSTR("%s at [%p], len=%d\n"), title, data, len);
+	printf_P(PSTR("%s at [%p], len=%d\r\n"), title, data, len);
 	ofs = 0;
 	while (ofs < len) {
 		/* format 1 line in the buffer, then use printk to print them */
@@ -232,10 +235,18 @@ int xbee_recv_data(struct xbee_recv_hdr *recvframe, unsigned len)
 		return -1;
 
 	switch (rch->type) {
-		case RC_PROTO_TYPE_CHANNEL:
+		case RC_PROTO_TYPE_CHANNEL: {
+			struct rc_proto_channel *rcc =
+				(struct rc_proto_channel *) recvframe->data;
+			int16_t val;
 			if (datalen != sizeof(struct rc_proto_channel))
 				return -1;
+			val = ntohs(rcc->axis[0]);
+			val >>= 6;
+			val += 512;
+			spi_servo_set(0, val);
 			break;
+				  }
 		case RC_PROTO_TYPE_RANGE: {
 			struct rc_proto_range *rcr =
 				(struct rc_proto_range *) recvframe->data;
@@ -497,24 +508,35 @@ void xbee_unload_timeout(struct xbee_ctx *ctx)
 
 void bootloader(void)
 {
-#define BOOTLOADER_ADDR 0x1e000
+#ifndef USE_USB
+#define BOOTLOADER_ADDR 0x3f000
 	if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
 		printf_P(PSTR("Bootloader is not present\r\n"));
 		return;
 	}
 	cli();
 	/* ... very specific :( */
+	TIMSK0 = 0;
+	TIMSK1 = 0;
+	TIMSK2 = 0;
+	TIMSK3 = 0;
+	TIMSK4 = 0;
+	TIMSK5 = 0;
 	EIMSK = 0;
+	UCSR0B = 0;
+	UCSR1B = 0;
+	UCSR2B = 0;
+	UCSR3B = 0;
 	SPCR = 0;
 	TWCR = 0;
 	ACSR = 0;
 	ADCSRA = 0;
 
-	__asm__ __volatile__ ("ldi r31,0xf0\n");
+	EIND = 1;
+	__asm__ __volatile__ ("ldi r31,0xf8\n");
 	__asm__ __volatile__ ("ldi r30,0x00\n");
-	__asm__ __volatile__ ("ijmp\n");
-
-	/* never returns */
+	__asm__ __volatile__ ("eijmp\n");
+#endif
 }
 
 void xbee_mainloop(void)
@@ -526,13 +548,12 @@ void xbee_mainloop(void)
 			int16_t c;
 
 			/* from xbee to cmdline */
-			c = CDC_Device_ReceiveByte(&VirtualSerial2_CDC_Interface);
+			c = xbee_dev_recv(NULL);
 			if (c >= 0)
-				CDC_Device_SendByte(&VirtualSerial1_CDC_Interface,
-						    (uint8_t)c);
+				cmdline_dev_send((uint8_t)c, NULL);
 
 			/* from cmdline to xbee */
-			c = CDC_Device_ReceiveByte(&VirtualSerial1_CDC_Interface);
+			c = cmdline_dev_recv(NULL);
 			if (c == 4) { /* CTRL-d */
 				xbee_raw = 0;
 				rdline_newline(&xbeeboard.rdl,
@@ -540,11 +561,10 @@ void xbee_mainloop(void)
 			}
 			else if (c >= 0) {
 				/* send to xbee */
-				CDC_Device_SendByte(&VirtualSerial2_CDC_Interface,
-						    (uint8_t)c);
+				xbee_dev_send((uint8_t)c, NULL);
+
 				/* echo on cmdline */
-				CDC_Device_SendByte(&VirtualSerial1_CDC_Interface,
-						    (uint8_t)c);
+				cmdline_dev_send((uint8_t)c, NULL);
 			}
 		}
 		else {
@@ -553,8 +573,107 @@ void xbee_mainloop(void)
 			xbee_proto_rx(xbee_dev);
 		}
 
+#ifdef USE_USB
 		CDC_Device_USBTask(&VirtualSerial1_CDC_Interface);
 		CDC_Device_USBTask(&VirtualSerial2_CDC_Interface);
 		USB_USBTask();
+#endif
 	}
 }
+
+/* return time in milliseconds on unsigned 16 bits */
+static uint16_t get_time_ms(void)
+{
+	return global_ms;
+}
+
+static void do_led_blink(struct callout_manager *cm,
+			 struct callout *clt, void *dummy)
+{
+	static uint8_t a = 0;
+
+#ifdef USE_USB
+	if (a & 1)
+		LEDs_SetAllLEDs(0);
+	else
+		LEDs_SetAllLEDs(0xff);
+#else
+	/* XXX */
+#endif
+	a++;
+}
+
+static void increment_ms(void *dummy)
+{
+	global_ms++;
+}
+
+static void main_timer_interrupt(void)
+{
+	static uint8_t cpt = 0;
+	cpt++;
+	sei();
+	if ((cpt & 0x3) == 0)
+		scheduler_interrupt();
+}
+
+/** Main program entry point. This routine contains the overall program flow, including initial
+ *  setup of all components and the main program loop.
+ */
+int main(void)
+{
+	struct callout t1;
+	FILE *xbee_file;
+	int8_t err;
+	struct xbee_dev dev;
+
+#ifdef USE_USB
+	SetupHardware();
+
+	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
+#else
+	uart_init();
+	uart_register_rx_event(CMDLINE_UART, emergency);
+#endif
+
+	fdevopen(cmdline_dev_send, cmdline_dev_recv);
+	xbee_file = fdevopen(xbee_dev_send, xbee_dev_recv);
+	scheduler_init();
+	timer_init();
+	timer0_register_OV_intr(main_timer_interrupt);
+
+	scheduler_add_periodical_event_priority(increment_ms, NULL,
+						1000L / SCHEDULER_UNIT,
+						LED_PRIO);
+	cmdline_init();
+	spi_servo_init();
+#ifndef USE_USB
+	/* in usb mode, it's done in usb callback */
+	printf_P(PSTR("\r\n"));
+	rdline_newline(&xbeeboard.rdl, xbeeboard.prompt);
+#endif
+	callout_manager_init(&cm, get_time_ms);
+	callout_reset(&cm, &t1, 500, PERIODICAL, do_led_blink, NULL);
+
+	/* initialize libxbee */
+	err = xbee_init();
+	if (err < 0)
+		return -1;
+
+	xbee_dev = &dev;
+
+	/* open xbee device */
+	if (xbee_open(xbee_dev, xbee_file) < 0)
+		return -1;
+
+	/* register default channel with a callback */
+	if (xbee_register_channel(xbee_dev, XBEE_DEFAULT_CHANNEL,
+				  xbee_rx, NULL) < 0) {
+		fprintf(stderr, "cannot register default channel\n");
+		return -1;
+	}
+
+	sei();
+	xbee_mainloop();
+	return 0;
+}