From 2969e16f7d55a17ebc404b4d6fe2920ad51cfc0e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Mar 2012 20:34:24 +1100 Subject: [PATCH] RC: disable interrupts when reading the RC registers this prevents getting bogus values which could cause a flight mode change --- libraries/APM_RC/APM_RC_APM1.cpp | 30 +++++++++++++++--------------- libraries/APM_RC/APM_RC_APM2.cpp | 25 +++++++++++++------------ 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index add4693172..4370378178 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -193,24 +193,24 @@ void APM_RC_APM1::disable_out(uint8_t ch) uint16_t APM_RC_APM1::InputCh(uint8_t ch) { - uint16_t result; + uint16_t result; - if (_HIL_override[ch] != 0) { - return _HIL_override[ch]; - } + if (_HIL_override[ch] != 0) { + return _HIL_override[ch]; + } - // Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted. - // We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct... - result = _PWM_RAW[ch]; - if (result != _PWM_RAW[ch]) { - result = _PWM_RAW[ch]; // if the results are different we make a third reading (this should be fine) - } - result >>= 1; // Because timer runs at 0.5us we need to do value/2 + // we need to stop interrupts to be sure we get a correct 16 bit value + cli(); + result = _PWM_RAW[ch]; + sei(); - // Limit values to a valid range - result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); - _radio_status=0; // Radio channel read - return(result); + // Because timer runs at 0.5us we need to do value/2 + result >>= 1; + + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + _radio_status = 0; // Radio channel read + return result; } uint8_t APM_RC_APM1::GetState(void) diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp index c76c71c08c..0a728cf4f4 100644 --- a/libraries/APM_RC/APM_RC_APM2.cpp +++ b/libraries/APM_RC/APM_RC_APM2.cpp @@ -20,6 +20,7 @@ */ #include "APM_RC_APM2.h" +#include #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else @@ -194,24 +195,24 @@ void APM_RC_APM2::disable_out(uint8_t ch) uint16_t APM_RC_APM2::InputCh(unsigned char ch) { - uint16_t result; - uint16_t result2; + uint16_t result; if (_HIL_override[ch] != 0) { return _HIL_override[ch]; } - // Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted. - // We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct... - result = _PWM_RAW[ch]>>1; // Because timer runs at 0.5us we need to do value/2 - result2 = _PWM_RAW[ch]>>1; - if (result != result2) - result = _PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) + // we need to block interrupts during the read of a 16 bit + // value + cli(); + result = _PWM_RAW[ch]; + sei(); + // Because timer runs at 0.5us we need to do value/2 + result >>= 1; - // Limit values to a valid range - result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); - _radio_status=0; // Radio channel read - return(result); + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + _radio_status = 0; // Radio channel read + return result; } unsigned char APM_RC_APM2::GetState(void)