From cc6ac756becd992b51ddc7127846ff0fc3d4c662 Mon Sep 17 00:00:00 2001
From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 15 Oct 2013 21:32:20 +0200
Subject: [PATCH] new spi protocol that supports PPM generation

---
 commands.c  | 15 +++++++++++++-
 spi_servo.c | 59 +++++++++++++++++++++++++++++++----------------------
 spi_servo.h |  1 +
 3 files changed, 50 insertions(+), 25 deletions(-)

diff --git a/commands.c b/commands.c
index 4389a6b..083f85f 100644
--- a/commands.c
+++ b/commands.c
@@ -1313,6 +1313,7 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 	(void)data;
 
 	spi_servo_bypass(0);
+	spi_servo_ppm(0);
 
 	/* stress test: send many commands, no wait between each servo
 	 * of a series, and a variable delay between series */
@@ -1365,9 +1366,21 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 		printf_P(PSTR("\r\n"));
 	}
 
+	printf_P(PSTR("PPM to servo\r\n"));
 	spi_servo_bypass(0);
+	spi_servo_ppm(0);
 
-	printf_P(PSTR("PPM to servo\r\n"));
+	/* test PPM to servo (bypass) mode */
+	while (!cmdline_keypressed()) {
+		for (i = 0; i < 6; i++) {
+			val = spi_servo_get(i);
+			spi_servo_set(i, val);
+		}
+	}
+
+	printf_P(PSTR("PPM to (servo + PPM)\r\n"));
+	spi_servo_bypass(0);
+	spi_servo_ppm(1);
 
 	/* test PPM to servo (bypass) mode */
 	while (!cmdline_keypressed()) {
diff --git a/spi_servo.c b/spi_servo.c
index 69afed2..6e1da05 100644
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -28,10 +28,12 @@
 #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
 
 #define N_SERVO 6
-#define BYPASS_ENABLE 14
-#define BYPASS_DISABLE 15
+
+#define PPM_BIT    0x01
+#define BYPASS_BIT 0x02
+
 struct spi_servo_tx {
-	uint16_t servo[N_SERVO];
+	uint16_t servo[N_SERVO+1]; /* one more for control channel */
 	uint16_t cmd_mask;
 	uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
 	uint8_t cur_idx;
@@ -51,9 +53,11 @@ static struct spi_servo_rx spi_servo_rx;
  * 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.
+ * Command 0 is only one byte long, it means "I have nothing to say".
+ * Commands 1 to N_SERVO (included) are to set the value of servo.
+ * Command N_SERVO+1 is:
+ * - to enable/disable ppm generation in place of last servo.
+ * - to enable/disable bypass mode
  */
 union spi_byte0 {
 	uint8_t u8;
@@ -93,10 +97,7 @@ static void spi_send_one_servo(uint8_t num, uint16_t val)
 	union spi_byte1 byte1;
 
 	byte0.val_msb = val >> 7;
-	if (num < N_SERVO)
-		byte0.cmd_num = num + 1;
-	else
-		byte0.cmd_num = num;
+	byte0.cmd_num = num + 1;
 	byte0.zero = 0;
 	byte1.one = 1;
 	byte1.val_lsb = val;
@@ -161,9 +162,7 @@ static void spi_servo_cb(void *arg)
 	idx = spi_servo_tx.cur_idx;
 	while (1) {
 		idx++;
-		if (idx == N_SERVO)
-			idx = BYPASS_ENABLE;
-		else if (idx >= sizeof(uint16_t) * 8)
+		if (idx > N_SERVO + 1)
 			idx = 0;
 
 		if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
@@ -190,7 +189,7 @@ void spi_servo_init(void)
 
 	scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
 					    SPI_EVT_PERIOD, SPI_PRIO);
-	spi_servo_set(BYPASS_DISABLE, 0);
+	spi_servo_bypass(1);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
@@ -225,14 +224,26 @@ void spi_servo_bypass(uint8_t enable)
 {
 	uint8_t flags;
 
-	if (enable) {
-		IRQ_LOCK(flags);
-		spi_servo_tx.cmd_mask |= (1 << BYPASS_ENABLE);
-		IRQ_UNLOCK(flags);
-	}
-	else {
-		IRQ_LOCK(flags);
-		spi_servo_tx.cmd_mask |= (1 << BYPASS_DISABLE);
-		IRQ_UNLOCK(flags);
-	}
+	IRQ_LOCK(flags);
+	spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+	if (enable)
+		spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
+	else
+		spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
+	spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+	IRQ_UNLOCK(flags);
+}
+
+void spi_servo_ppm(uint8_t enable)
+{
+	uint8_t flags;
+
+	IRQ_LOCK(flags);
+	spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+	if (enable)
+		spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
+	else
+		spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
+	spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+	IRQ_UNLOCK(flags);
 }
diff --git a/spi_servo.h b/spi_servo.h
index cca0b76..7156166 100644
--- a/spi_servo.h
+++ b/spi_servo.h
@@ -2,3 +2,4 @@ void spi_servo_init(void);
 void spi_servo_set(uint8_t num, uint16_t val);
 uint16_t spi_servo_get(uint8_t num);
 void spi_servo_bypass(uint8_t enable);
+void spi_servo_ppm(uint8_t enable);
-- 
2.39.5