From: Fabrice Desclaux <serpilliere@droids-corp.org>
Date: Tue, 3 Apr 2012 19:20:40 +0000 (+0200)
Subject: control servo 0 with axis 0
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=9a9b64112aee5ab26398b46cb13b7e49c292a355;p=protos%2Fxbee-avr.git

control servo 0 with axis 0
---

diff --git a/main.c b/main.c
index b9fa0a8..e9454c4 100644
--- 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 1bb96e7..fad6866 100644
--- 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"