diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp index 4fc16046b9..8de0b5ebe3 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp @@ -12,7 +12,8 @@ using namespace AVR_SITL; extern const AP_HAL::HAL& hal; -ADCSource::ADCSource(uint8_t pin, float prescale) : +ADCSource::ADCSource(SITL_State *sitlState, uint8_t pin, float prescale) : + _sitlState(sitlState), _pin(pin), _prescale(prescale) {} @@ -25,6 +26,10 @@ float ADCSource::read_latest() { switch (_pin) { case ANALOG_INPUT_BOARD_VCC: return 4900; + + case 0: + return _sitlState->airspeed_pin_value; + case ANALOG_INPUT_NONE: default: return 0.0; @@ -35,8 +40,6 @@ void ADCSource::set_pin(uint8_t pin) { _pin = pin; } -SITLAnalogIn::SITLAnalogIn() {} - void SITLAnalogIn::init(void *ap_hal_scheduler) { } @@ -45,7 +48,7 @@ AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) { } AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin, float prescale) { - return new ADCSource(pin, prescale); + return new ADCSource(_sitlState, pin, prescale); } #endif diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.h b/libraries/AP_HAL_AVR_SITL/AnalogIn.h index eb8fd84da7..51a0323b40 100644 --- a/libraries/AP_HAL_AVR_SITL/AnalogIn.h +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.h @@ -12,7 +12,7 @@ public: friend class AVR_SITL::SITLAnalogIn; /* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC, * board vcc */ - ADCSource(uint8_t pin, float prescale = 1.0); + ADCSource(SITL_State *sitlState, uint8_t pin, float prescale = 1.0); /* implement AnalogSource virtual api: */ float read_average(); @@ -21,6 +21,7 @@ public: private: /* prescale scales the raw measurments for read()*/ + SITL_State *_sitlState; uint8_t _pin; const float _prescale; }; @@ -29,14 +30,16 @@ private: * timer event and the AP_HAL::AnalogIn interface */ class AVR_SITL::SITLAnalogIn : public AP_HAL::AnalogIn { public: - SITLAnalogIn(); + SITLAnalogIn(SITL_State *sitlState) { + _sitlState = sitlState; + } void init(void* ap_hal_scheduler); AP_HAL::AnalogSource* channel(int16_t n); AP_HAL::AnalogSource* channel(int16_t n, float prescale); private: static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS]; - + SITL_State *_sitlState; }; #endif // __AP_HAL_AVR_SITL_ANALOG_IN_H__ diff --git a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp index 324816d112..af52587e56 100644 --- a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp +++ b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp @@ -29,7 +29,7 @@ static SITLConsoleDriver consoleDriver; static SITL_State sitlState; static SITLRCInput sitlRCInput(&sitlState); static SITLRCOutput sitlRCOutput(&sitlState); -static SITLAnalogIn sitlAnalogIn; +static SITLAnalogIn sitlAnalogIn(&sitlState); static Empty::EmptyGPIO emptyGPIO; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index fc0048e039..6dfb8405ec 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -31,6 +31,7 @@ struct sockaddr_in SITL_State::_rcout_addr; pid_t SITL_State::_parent_pid; uint32_t SITL_State::_update_count; bool SITL_State::_motors_on; +uint16_t SITL_State::airspeed_pin_value; AP_Baro_BMP085_HIL *SITL_State::_barometer; AP_InertialSensor_Stub *SITL_State::_ins; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index 0590ff9314..5338d877d0 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -39,6 +39,9 @@ public: static uint16_t pwm_input[8]; static void loop_hook(void); + // simulated airspeed + static uint16_t airspeed_pin_value; + private: void _parse_command_line(int argc, char * const argv[]); void _usage(void); diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index 185c99e444..004f2bd90a 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -119,6 +119,8 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative _ins->set_gyro(Vector3f(p, q, r)); _ins->set_accel(Vector3f(xAccel, yAccel, zAccel)); + + airspeed_pin_value = _airspeed_sensor(airspeed); } #endif