From 97d85de36111c5b01b8854e4132fca735cfc17c4 Mon Sep 17 00:00:00 2001
From: John Arne Birkeland <johnarne@gmail.com>
Date: Thu, 8 Nov 2012 01:03:24 +0100
Subject: [PATCH] Optimized APM_RC.InputCh() to prevent global interrupt
 disable each time a value is read.

---
 libraries/APM_RC/APM_RC_APM1.cpp | 11 ++++++++---
 libraries/APM_RC/APM_RC_APM2.cpp | 11 ++++++++---
 2 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp
index ca8f635452..0b86be58f9 100644
--- a/libraries/APM_RC/APM_RC_APM1.cpp
+++ b/libraries/APM_RC/APM_RC_APM1.cpp
@@ -218,10 +218,15 @@ uint16_t APM_RC_APM1::InputCh(uint8_t ch)
         return _HIL_override[ch];
     }
 
-    // we need to stop interrupts to be sure we get a correct 16 bit value
-    cli();
+    // We need to block ICP4 interrupts during the read of 16 bit PWM values
+    uint8_t _timsk4 = TIMSK4;
+    TIMSK4 &= ~(1<<ICIE4);
+
+    // value
     result = _PWM_RAW[ch];
-    sei();
+
+   // Enable ICP4 interrupt if previously active
+    TIMSK4 = _timsk4;
 
     // Because timer runs at 0.5us we need to do value/2
     result >>= 1;
diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp
index 811d16c181..3ad1abff5e 100644
--- a/libraries/APM_RC/APM_RC_APM2.cpp
+++ b/libraries/APM_RC/APM_RC_APM2.cpp
@@ -240,11 +240,16 @@ uint16_t APM_RC_APM2::InputCh(unsigned char ch)
         return _HIL_override[ch];
     }
 
-    // we need to block interrupts during the read of a 16 bit
+    // We need to block ICP5 interrupts during the read of 16 bit PWM values
+    uint8_t _timsk5 = TIMSK5;
+    TIMSK5 &= ~(1<<ICIE5);
+
     // value
-    cli();
     result = _PWM_RAW[ch];
-    sei();
+
+    // Enable ICP5 interrupt if previously active
+    TIMSK5 = _timsk5;
+    
     // Because timer runs at 0.5us we need to do value/2
     result >>= 1;