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

diff --git a/main.c b/main.c
index f06bc44..74f6629 100644
--- 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++;