add PPM generation support and new spi protocol

diff --git a/main.c b/main.c
index 74f6629fb8c4f1d7ee64ee01334640483ffa1201..81ac65b435fe5b2b2fb7b014a691c4930df4184e 100644 (file)
--- a/main.c
+++ b/main.c
@@ -32,21 +32,25 @@ static struct servo servo_table[] = {
                .command = 512,
        },
 };
-#define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
+#define N_SERVO (sizeof(servo_table)/sizeof(*servo_table))
+
+/* we use the first servo for PPM output if enabled */
+#define PPM (0)
 
 static uint8_t bypass;
+static uint8_t ppm_enabled;
 static volatile uint8_t done;
 static uint8_t portval;
 static uint8_t rxidx;
 
-static uint8_t icp_idx = NB_SERVO;
-static uint16_t icp_servos[NB_SERVO];
+static uint8_t icp_idx = N_SERVO;
+static uint16_t icp_servos[N_SERVO];
 static uint16_t icp_prev;
 
 static uint8_t spi_out_idx; /* current byte beeing sent */
 
-#define BYPASS_ENABLE 14
-#define BYPASS_DISABLE 15
+#define PPM_BIT    0x01
+#define BYPASS_BIT 0x02
 
 #define LED_ON() do { PORTB |= 0x02; } while(0)
 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
@@ -60,9 +64,10 @@ static uint8_t spi_out_idx; /* current byte beeing sent */
  * servo value. The second byte contains the lsb of the servo value.
  *
  * Command 0 is only one byte long, it means "I have nothing to say".
- * Commands 1 to NB_SERVO+1 are to set the value of servo.
- * Command 14 is to enable bypass mode.
- * Command 15 is to disable bypass mode.
+ * 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 first servo.
+ * - to enable/disable bypass mode
  */
 union byte0 {
        uint8_t u8;
@@ -120,7 +125,7 @@ static void poll_spi(void)
                SPDR = byte1_tx.u8;
        }
        spi_out_idx ++;
-       if (spi_out_idx >= NB_SERVO * 2)
+       if (spi_out_idx >= N_SERVO * 2)
                spi_out_idx = 0;
 
        /* RX */
@@ -148,16 +153,21 @@ static void poll_spi(void)
 
                /* process command */
 
-               if (byte0_rx.cmd_num < NB_SERVO+1) {
-                       val = (uint16_t)byte0_rx.val_msb << 7;
-                       val += byte1_rx.val_lsb;
+               val = (uint16_t)byte0_rx.val_msb << 7;
+               val += byte1_rx.val_lsb;
+
+               if (byte0_rx.cmd_num < N_SERVO+1) {
                        servo_table[byte0_rx.cmd_num-1].command = val;
                }
-               else if (byte0_rx.cmd_num == BYPASS_ENABLE) {
-                       bypass = 1;
-               }
-               else if (byte0_rx.cmd_num == BYPASS_DISABLE) {
-                       bypass = 0;
+               else if (byte0_rx.cmd_num == N_SERVO+1) {
+                       if (val & PPM_BIT)
+                               ppm_enabled = 1;
+                       else
+                               ppm_enabled = 0;
+                       if (val & BYPASS_BIT)
+                               bypass = 1;
+                       else
+                               bypass = 0;
                }
        }
 
@@ -190,7 +200,7 @@ static void poll_input_capture(void)
        }
 
        /* get the value for the servo */
-       if (icp_idx < NB_SERVO) {
+       if (icp_idx < N_SERVO) {
                if (diff < 1000)
                        icp_servos[icp_idx] = 0;
                else if (diff > 2023)
@@ -213,31 +223,62 @@ static void load_timer_at(uint16_t t)
        TIMSK1 |= _BV(OCIE1A);
 }
 
-static void do_one_servo(struct servo *s)
+static void do_servos(void)
 {
-       uint16_t t;
+       uint8_t i;
+       uint16_t t, start;
+
+       /* skip first servo if ppm is enabled */
+       if (ppm_enabled)
+               i = 1;
+       else
+               i = 0;
 
-       /* set bit */
-       done = 0;
-       //portval = PORTC | (1 << s->bit);
-       portval = (1 << s->bit);
        t = TCNT1;
-       load_timer_at(t + 20);
+       start = t + 100;
+
+       for (; i < N_SERVO; i++) {
+
+               /* set servo and PPM bit */
+               portval = 1 << servo_table[i].bit;
+               if (ppm_enabled)
+                       portval |= (1 << servo_table[PPM].bit);
+
+               done = 0;
+               load_timer_at(start);
+               while (done == 0)
+                       poll();
+
+               /* reset PPM bit after 300us */
+               portval = 1 << servo_table[i].bit;
+               done = 0;
+               load_timer_at(start + 300);
+               while (done == 0)
+                       poll();
+
+               start = start + 1000 + servo_table[i].command;
+       }
+
+       /* set PPM bit only for last servo */
+       portval = 0;
+       if (ppm_enabled)
+               portval |= (1 << servo_table[PPM].bit);
+
+       done = 0;
+       load_timer_at(start);
        while (done == 0)
                poll();
 
-       /* reset bit */
-       done = 0;
+       /* reset PPM bit after 300us */
        portval = 0;
-       //portval = PORTC & (~(1 << s->bit));
-       load_timer_at(t + 20 + 1000 + s->command);
+       done = 0;
+       load_timer_at(start + 300);
        while (done == 0)
                poll();
 }
 
 int main(void)
 {
-       uint8_t i;
        uint8_t t, diff;
        uint8_t tmp;
        uint8_t cnt = 10;
@@ -280,9 +321,7 @@ int main(void)
        bypass = 0;
        while (1) {
                t = TCNT0;
-               for (i = 0; i < NB_SERVO; i++) {
-                       do_one_servo(&servo_table[i]);
-               }
+               do_servos();
                /* wait 20 ms */
                while (1) {
                        diff = TCNT0 - t;