X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=spi_servo.c;fp=spi_servo.c;h=1bb96e73959ab4aaf4eb1226f6848bf8c08662b3;hp=0000000000000000000000000000000000000000;hb=f97044f0c5ecbbc665bd7ac1d454fd9479476a78;hpb=50f42a7b2f1a2be5be2a2e4207e304ce046d1965

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);
+}