diff --git a/libraries/AP_HAL_SITL/AnalogIn.cpp b/libraries/AP_HAL_SITL/AnalogIn.cpp index 50ed481ee1..265092a072 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_SITL/AnalogIn.cpp @@ -5,6 +5,8 @@ #include "AnalogIn.h" #include +#define VOLTAGE_TO_PIN_VALUE(_v) (constrain_float(_v * (SITL_ADC_MAX_PIN_VALUE/SITL_ADC_FULL_SCALE_VOLTAGE), 0, SITL_ADC_MAX_PIN_VALUE)) + using namespace HALSITL; extern const AP_HAL::HAL& hal; @@ -19,38 +21,34 @@ float ADCSource::read_average() { } float ADCSource::voltage_average() { - return (5.0f/1023.0f) * read_average(); + return voltage_latest(); } float ADCSource::voltage_latest() { - return (5.0f/1023.0f) * read_latest(); -} - -float ADCSource::read_latest() { switch (_pin) { case ANALOG_INPUT_BOARD_VCC: - return 1023; + return SITL_ADC_MAX_PIN_VALUE; case 0: - return _sitlState->sonar_pin_value; + return _sitlState->sonar_pin_voltage; case 1: - return _sitlState->airspeed_pin_value[0]; + return _sitlState->airspeed_pin_voltage[0]; case 2: - return _sitlState->airspeed_pin_value[1]; + return _sitlState->airspeed_pin_voltage[1]; case 12: - return _sitlState->current_pin_value; + return _sitlState->current_pin_voltage; case 13: - return _sitlState->voltage_pin_value; + return _sitlState->voltage_pin_voltage; case 14: - return _sitlState->current2_pin_value; + return _sitlState->current2_pin_voltage; case 15: - return _sitlState->voltage2_pin_value; + return _sitlState->voltage2_pin_voltage; case ANALOG_INPUT_NONE: default: @@ -58,6 +56,10 @@ float ADCSource::read_latest() { } } +float ADCSource::read_latest() { + return VOLTAGE_TO_PIN_VALUE(voltage_latest()); +} + bool ADCSource::set_pin(uint8_t pin) { _pin = pin; return true; diff --git a/libraries/AP_HAL_SITL/AnalogIn.h b/libraries/AP_HAL_SITL/AnalogIn.h index ae529705da..40d6b52494 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.h +++ b/libraries/AP_HAL_SITL/AnalogIn.h @@ -3,6 +3,9 @@ #include #include "AP_HAL_SITL_Namespace.h" +#define SITL_ADC_RESOLUTION 16 // bits of resolution +#define SITL_ADC_MAX_PIN_VALUE ((1<state.airspeed_raw_pressure[i] = airspeed_pressure; - - airspeed_pin_value[i] = MIN(0xFFFF, airspeed_raw / 4); + + airspeed_pin_voltage[i] = PASCAL_TO_VOLTS(airspeed_raw); } } diff --git a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp index 93fb75fab6..5a930db4ea 100644 --- a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp +++ b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp @@ -51,7 +51,7 @@ float SITL_State::_sonar_pin_voltage() const */ void SITL_State::_update_rangefinder() { - sonar_pin_value = 1023 * (_sonar_pin_voltage() / 5.0f); + sonar_pin_voltage = _sonar_pin_voltage(); } #endif