From 5f1bd1a45234458332b99edbe17cc2b9ccb29459 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 22 Apr 2013 11:36:00 +1000 Subject: [PATCH] HAL_AVR: constrain Vcc used in voltage_average() this ensures a bad Vcc reading doesn't throw off analog inputs too much --- libraries/AP_HAL_AVR/AnalogIn_ADC.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 035cb347b6..3f91746930 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -44,6 +44,13 @@ float ADCSource::voltage_average(void) { float vcc_mV = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC)->read_average(); float v = read_average(); + // constrain Vcc reading so that a bad Vcc doesn't throw off + // the reading of other sources too badly + if (vcc_mV < 4000) { + vcc_mV = 4000; + } else if (vcc_mV > 6000) { + vcc_mV = 6000; + } return v * vcc_mV * 9.765625e-7; // 9.765625e-7 = 1.0/(1024*1000) }