X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=main.c;h=74f6629fb8c4f1d7ee64ee01334640483ffa1201;hb=4bf4e06a310fc4177647dd0f4785ddaf1278d218;hp=923a8ea1bcd4ce553b2b089644768e77294f0b6c;hpb=516dc48d0a1c47e354856d3f71b2e5bd0454cf4e;p=protos%2Frc_servos.git

diff --git a/main.c b/main.c
index 923a8ea..74f6629 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 */
@@ -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++;