From f97044f0c5ecbbc665bd7ac1d454fd9479476a78 Mon Sep 17 00:00:00 2001
From: Fabrice Desclaux <serpilliere@droids-corp.org>
Date: Thu, 29 Mar 2012 22:20:23 +0200
Subject: [PATCH] spi support

---
 commands.c  | 44 +++++++++++++++++++++++++++
 main.c      |  2 ++
 makefile    |  1 +
 spi_servo.c | 87 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 spi_servo.h |  3 ++
 5 files changed, 137 insertions(+)
 create mode 100644 spi_servo.c
 create mode 100644 spi_servo.h

diff --git a/commands.c b/commands.c
index a14d098..6eb034c 100644
--- a/commands.c
+++ b/commands.c
@@ -29,6 +29,7 @@
 #include <aversive/queue.h>
 #include <aversive/endian.h>
 #include <aversive/error.h>
+#include <aversive/wait.h>
 #include <parse.h>
 #include <rdline.h>
 #include <parse_string.h>
@@ -46,6 +47,7 @@
 #include "parse_neighbor.h"
 #include "parse_monitor.h"
 
+#include "spi_servo.h"
 #include "rc_proto.h"
 #include "main.h"
 #include "cmdline.h"
@@ -1258,6 +1260,47 @@ parse_pgm_inst_t cmd_baudrate = {
 };
 #endif
 
+/* this structure is filled when cmd_test_spi is parsed successfully */
+struct cmd_test_spi_result {
+	fixed_string_t arg0;
+};
+
+static void cmd_test_spi_parsed(void * parsed_result,
+				__attribute__((unused)) void *data)
+{
+	int i;
+
+
+	while (1) {
+		for (i = 0; i < 50; i++) {
+			spi_servo_set(0, 0);
+			wait_ms(100);
+			spi_servo_set(0, 500);
+			wait_ms(100);
+		}
+
+		spi_servo_bypass(1);
+		wait_ms(10000);
+		spi_servo_bypass(0);
+		wait_ms(1);
+	}
+}
+
+prog_char str_test_spi_arg0[] = "test_spi";
+parse_pgm_token_string_t cmd_test_spi_arg0 =
+	TOKEN_STRING_INITIALIZER(struct cmd_test_spi_result, arg0,
+				 str_test_spi_arg0);
+
+prog_char help_test_spi[] = "Test the spi";
+parse_pgm_inst_t cmd_test_spi = {
+	.f = cmd_test_spi_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_test_spi,
+	.tokens = {        /* token list, NULL terminated */
+		(prog_void *)&cmd_test_spi_arg0,
+		NULL,
+	},
+};
 
 /* in progmem */
 parse_pgm_ctx_t main_ctx[] = {
@@ -1297,5 +1340,6 @@ parse_pgm_ctx_t main_ctx[] = {
 #ifndef USE_USB
 	(parse_pgm_inst_t *)&cmd_baudrate,
 #endif
+	(parse_pgm_inst_t *)&cmd_test_spi,
 	NULL,
 };
diff --git a/main.c b/main.c
index 5f7a638..d12ed7a 100644
--- 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;
@@ -618,6 +619,7 @@ int main(void)
 	int8_t err;
 	struct xbee_dev dev;
 
+	spi_servo_init();
 #ifdef USE_USB
 	SetupHardware();
 
diff --git a/makefile b/makefile
index 51feddd..fa48586 100644
--- a/makefile
+++ b/makefile
@@ -145,6 +145,7 @@ endif
 
 # List C source files here. (C dependencies are automatically generated.)
 SRC += \
+	spi_servo.c \
 	cirbuf_add_buf_head.c \
 	cirbuf_add_buf_tail.c \
 	cirbuf_add_head.c \
diff --git a/spi_servo.c b/spi_servo.c
new file mode 100644
index 0000000..1bb96e7
--- /dev/null
+++ b/spi_servo.c
@@ -0,0 +1,87 @@
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <stdint.h>
+
+#include "spi_servo.h"
+
+#define BYPASS_ENABLE 14
+#define BYPASS_DISABLE 15
+
+/*
+ * SPI protocol:
+ *
+ * A command is stored on 2 bytes. The first one has its msb to 0, and the
+ * second one to 1. The first received byte contains the command number, and the
+ * msb of the servo value. The second byte contains the lsb of the servo value.
+ *
+ * Commands 0 to NB_SERVO are to set the value of servo.
+ * Command 14 is to enable bypass mode.
+ * Command 15 is to disable bypass mode.
+ */
+static volatile union {
+	uint8_t u8;
+	struct {
+		/* inverted: little endian */
+		uint8_t val_msb:3;
+		uint8_t cmd_num:4;
+		uint8_t zero:1;
+	};
+} byte0;
+
+static volatile union {
+	uint8_t u8;
+	struct {
+		/* inverted: little endian */
+		uint8_t val_lsb:7;
+		uint8_t one:1;
+	};
+} byte1;
+
+void spi_servo_init(void)
+{
+	/* real SS ! */
+	DDRK = 0x2;
+	/* SCK, SS & MOSI */
+	DDRB = 0x7;
+
+	/* remove power reduction on spi */
+	PRR0 &= ~(1 << PRSPI);
+
+	/* Enable SPI, Master, set clock rate fck/64 */
+	SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
+
+	PORTK |= (1<<1);
+
+	spi_servo_set(BYPASS_DISABLE, 0);
+}
+
+void spi_servo_set(uint8_t num, uint16_t val)
+{
+	byte0.val_msb = val >> 7;
+	byte0.cmd_num = num;
+	byte0.zero = 0;
+	byte1.one = 1;
+	byte1.val_lsb = val;
+
+	PORTK &= ~(1<<1);
+	SPDR = byte0.u8;
+	/* Wait for transmission complete */
+	while(!(SPSR & (1<<SPIF)));
+	PORTK |= (1<<1);
+
+	_delay_loop_1(5);
+	PORTK &= ~(1<<1);
+
+	SPDR = byte1.u8;
+	/* Wait for transmission complete */
+	while(!(SPSR & (1<<SPIF)));
+	PORTK |= (1<<1);
+}
+
+void spi_servo_bypass(uint8_t enable)
+{
+	if (enable)
+		spi_servo_set(BYPASS_ENABLE, 0);
+	else
+		spi_servo_set(BYPASS_DISABLE, 0);
+}
diff --git a/spi_servo.h b/spi_servo.h
new file mode 100644
index 0000000..188f8cd
--- /dev/null
+++ b/spi_servo.h
@@ -0,0 +1,3 @@
+void spi_servo_init(void);
+void spi_servo_set(uint8_t num, uint16_t val);
+void spi_servo_bypass(uint8_t enable);
-- 
2.20.1