control servo 0 with axis 0

diff --git a/main.c b/main.c
index b9fa0a8c345c1a1bcedc653dbdf765e8333cce1e..e9454c405bf6902bff2a1ae3dd67f64d9f45119f 100644 (file)
--- a/main.c
+++ b/main.c
@@ -235,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;
@@ -619,7 +627,6 @@ int main(void)
        int8_t err;
        struct xbee_dev dev;
 
-       spi_servo_init();
 #ifdef USE_USB
        SetupHardware();
 
@@ -639,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"));

diff --git a/spi_servo.c b/spi_servo.c
index 1bb96e73959ab4aaf4eb1226f6848bf8c08662b3..fad6866ea25dda2407a712d1d8992a8ca7b6879f 100644 (file)
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -1,6 +1,7 @@
 #include <aversive.h>
 #include <aversive/wait.h>
 #include <stdint.h>
+#include <stdio.h>
 
 #include "spi_servo.h"