X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=main.c;h=f06bc44b4f92191d8be816fe52afd4c7a8d0ccb0;hb=5d39ce22b6dc5693c52e402491cd1dcaf968c6b4;hp=929ff1371ebf69adfce4ceaac0da19c3c4de03e2;hpb=0507a9ff89a8cb6be45ec26054d32eb238ec41be;p=protos%2Frc_servos.git

diff --git a/main.c b/main.c
index 929ff13..f06bc44 100644
--- 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 */
@@ -173,16 +173,16 @@ static void poll_input_capture(void)
 	if ((TIFR1 & _BV(ICF1)) == 0)
 		return;
 
-	sei();
-	icp = ICR1;
 	cli();
+	icp = ICR1;
+	sei();
 
-	rising = TCCR1B & _BV(ICES1);
+	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;
@@ -244,6 +244,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 +252,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;