Browse Source

HAL_AVR: constrain Vcc used in voltage_average()

this ensures a bad Vcc reading doesn't throw off analog inputs too
much
mission-4.1.18
Andrew Tridgell 12 years ago
parent
commit
5f1bd1a452
  1. 7
      libraries/AP_HAL_AVR/AnalogIn_ADC.cpp

7
libraries/AP_HAL_AVR/AnalogIn_ADC.cpp

@ -44,6 +44,13 @@ float ADCSource::voltage_average(void) @@ -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)
}

Loading…
Cancel
Save