fix ppm decoding

[protos/rc_servos.git] / main.c

diff --git a/main.c b/main.c
index 929ff13..74f6629 100644 (file)
--- a/main.c
+++ b/main.c
@@ -120,7 +120,7 @@ static void poll_spi(void)
                SPDR = byte1_tx.u8;
        }
        spi_out_idx ++;
-       if (spi_out_idx >= NB_SERVO)
+       if (spi_out_idx >= NB_SERVO * 2)
                spi_out_idx = 0;
 
        /* RX */
@@ -167,22 +167,16 @@ static void poll_spi(void)
 static void poll_input_capture(void)
 {
        uint16_t icp, diff;
-       uint8_t rising;
 
        /* no new sample, return */
        if ((TIFR1 & _BV(ICF1)) == 0)
                return;
 
-       sei();
-       icp = ICR1;
        cli();
+       icp = ICR1;
+       sei();
 
-       rising = TCCR1B & _BV(ICES1);
-
-       /* change the edge type */
-       TCCR1B ^= _BV(ICES1);
-
-       /* clear the flag */
+       /* clear the flag by writing a one */
        TIFR1 = TIFR1 | _BV(ICF1);
 
        diff = icp - icp_prev;
@@ -190,15 +184,17 @@ static void poll_input_capture(void)
 
        /* a rising edge with at least 2ms of state 0 means that we
         * get the first servo */
-       if (rising == 1 && diff > 2000) {
+       if (diff > 3000) {
                icp_idx = 0;
                return;
        }
 
        /* get the value for the servo */
-       if (rising == 0 && icp_idx < NB_SERVO) {
+       if (icp_idx < NB_SERVO) {
                if (diff < 1000)
                        icp_servos[icp_idx] = 0;
+               else if (diff > 2023)
+                       icp_servos[icp_idx] = 1023;
                else
                        icp_servos[icp_idx] = diff - 1000;
                icp_idx++;
@@ -244,6 +240,7 @@ int main(void)
        uint8_t i;
        uint8_t t, diff;
        uint8_t tmp;
+       uint8_t cnt = 10;
 
        /* use pull-up for inputs */
        PORTC |= 0x3f;
@@ -251,14 +248,15 @@ int main(void)
        /* LED */
        DDRB = 0x02;
 
-#if 0 /* LED debug */
-       while (1) {
+       while (cnt > 0) {
+#if 1 /* disable for LED debug only */
+               cnt--;
+#endif
                LED_ON();
                wait_ms(100);
                LED_OFF();
                wait_ms(100);
        }
-#endif
 
        /* servo outputs PD2-PD7 */
        DDRD = 0xfc;