send PPM input on SPI

Signed-off-by: Olivier Matz <zer0@droids-corp.org>

diff --git a/main.c b/main.c
index 9c368d5aa27b08f19da25d12c75249adc95f99ba..929ff1371ebf69adfce4ceaac0da19c3c4de03e2 100644 (file)
--- a/main.c
+++ b/main.c
@@ -43,6 +43,8 @@ static uint8_t icp_idx = NB_SERVO;
 static uint16_t icp_servos[NB_SERVO];
 static uint16_t icp_prev;
 
+static uint8_t spi_out_idx; /* current byte beeing sent */
+
 #define BYPASS_ENABLE 14
 #define BYPASS_DISABLE 15
 
@@ -62,7 +64,7 @@ static uint16_t icp_prev;
  * Command 14 is to enable bypass mode.
  * Command 15 is to disable bypass mode.
  */
-static union {
+union byte0 {
        uint8_t u8;
        struct {
                /* inverted: little endian */
@@ -70,16 +72,16 @@ static union {
                uint8_t cmd_num:4;
                uint8_t zero:1;
        };
-} byte0;
+};
 
-static union {
+union byte1 {
        uint8_t u8;
        struct {
                /* inverted: little endian */
                uint8_t val_lsb:7;
                uint8_t one:1;
        };
-} byte1;
+};
 
 SIGNAL(TIMER1_COMPA_vect)
 {
@@ -91,12 +93,38 @@ SIGNAL(TIMER1_COMPA_vect)
 static void poll_spi(void)
 {
        uint8_t c;
+       uint16_t servo;
+       static union byte0 byte0_rx;
+       union byte1 byte1_rx;
+       union byte0 byte0_tx;
+       static union byte1 byte1_tx;
 
        /* reception complete ? */
        if (!(SPSR & (1<<SPIF)))
                return;
 
        c = SPDR;
+
+       /* prepare next TX */
+
+       if ((spi_out_idx & 1) == 0) {
+               servo = icp_servos[spi_out_idx >> 1];
+               byte0_tx.val_msb = servo >> 7;
+               byte0_tx.cmd_num = (spi_out_idx >> 1) + 1;
+               byte0_tx.zero = 0;
+               byte1_tx.val_lsb = servo & 0x7f;
+               byte1_tx.one = 1;
+               SPDR = byte0_tx.u8;
+       }
+       else {
+               SPDR = byte1_tx.u8;
+       }
+       spi_out_idx ++;
+       if (spi_out_idx >= NB_SERVO)
+               spi_out_idx = 0;
+
+       /* RX */
+
        if ((rxidx == 0) && (c & 0x80)) {
                rxidx = 0;
                return; /* drop */
@@ -107,28 +135,28 @@ static void poll_spi(void)
        }
 
        if (rxidx == 0) {
-               byte0.u8 = c;
+               byte0_rx.u8 = c;
 
                /* command num 0 is ignored */
-               if (byte0.cmd_num == 0)
+               if (byte0_rx.cmd_num == 0)
                        return;
        }
        else {
                uint16_t val;
 
-               byte1.u8 = c;
+               byte1_rx.u8 = c;
 
                /* process command */
 
-               if (byte0.cmd_num < NB_SERVO+1) {
-                       val = (uint16_t)byte0.val_msb << 7;
-                       val += byte1.val_lsb;
-                       servo_table[byte0.cmd_num-1].command = val;
+               if (byte0_rx.cmd_num < NB_SERVO+1) {
+                       val = (uint16_t)byte0_rx.val_msb << 7;
+                       val += byte1_rx.val_lsb;
+                       servo_table[byte0_rx.cmd_num-1].command = val;
                }
-               else if (byte0.cmd_num == BYPASS_ENABLE) {
+               else if (byte0_rx.cmd_num == BYPASS_ENABLE) {
                        bypass = 1;
                }
-               else if (byte0.cmd_num == BYPASS_DISABLE) {
+               else if (byte0_rx.cmd_num == BYPASS_DISABLE) {
                        bypass = 0;
                }
        }
@@ -244,8 +272,10 @@ int main(void)
        TCNT0 = 0;
        TCCR0B = _BV(CS02) | _BV(CS00);
 
-       /* enable spi (don't set unused MISO as output) */
+       /* enable spi (set MISO as output) */
        SPCR = _BV(SPE);
+       SPDR = 0;
+       DDRB |= _BV(4);
 
        sei();