diff --git a/libraries/AP_HAL_AVR/AnalogIn.h b/libraries/AP_HAL_AVR/AnalogIn.h index f4456a8125..eb4922ecaa 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -19,6 +19,7 @@ public: float read_latest(); void set_pin(uint8_t p); float voltage_average(); + float voltage_latest(); float voltage_average_ratiometric(); void set_stop_pin(uint8_t p); void set_settle_time(uint16_t settle_time_ms); diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 958dc0a4b6..c0c8fed21c 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -60,6 +60,26 @@ float ADCSource::voltage_average(void) return v * vcc_mV * 9.765625e-7; // 9.765625e-7 = 1.0/(1024*1000) } +/* + return voltage from 0.0 to 5.0V, scaled to Vcc + */ +float ADCSource::voltage_latest(void) +{ + if (_pin == ANALOG_INPUT_BOARD_VCC) { + return read_latest() * 0.001f; + } + float vcc_mV = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC)->read_average(); + float v = read_latest(); + // 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) +} + /* return voltage from 0.0 to 5.0V, assuming a ratiometric sensor. This means the result is really a pseudo-voltage, that assumes the supply