fix ppm decoding

[protos/rc_servos.git] / main.c

diff --git a/main.c b/main.c
index f06bc44..74f6629 100644 (file)
--- a/main.c
+++ b/main.c
@@ -167,7 +167,6 @@ 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)
@@ -177,11 +176,6 @@ static void poll_input_capture(void)
        icp = ICR1;
        sei();
 
-       rising = !!(TCCR1B & _BV(ICES1));
-
-       /* change the edge type */
-       TCCR1B ^= _BV(ICES1);
-
        /* clear the flag by writing a one */
        TIFR1 = TIFR1 | _BV(ICF1);
 
@@ -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++;