control servo 0 with axis 0

[protos/xbee-avr.git] / main.c

diff --git a/main.c b/main.c
index ad94ba8..e9454c4 100644 (file)
--- a/main.c
+++ b/main.c
@@ -57,6 +57,7 @@
 #include "cmdline.h"
 #include "callout.h"
 #include "rc_proto.h"
+#include "spi_servo.h"
 #include "main.h"
 
 struct xbeeboard xbeeboard;
@@ -234,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;
@@ -499,24 +508,35 @@ void xbee_unload_timeout(struct xbee_ctx *ctx)
 
 void bootloader(void)
 {
-#define BOOTLOADER_ADDR 0x1f800
+#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)
@@ -613,6 +633,7 @@ int main(void)
        LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
 #else
        uart_init();
+       uart_register_rx_event(CMDLINE_UART, emergency);
 #endif
 
        fdevopen(cmdline_dev_send, cmdline_dev_recv);
@@ -625,6 +646,7 @@ int main(void)
                                                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"));