From 86afc26609766febd7e7d8f09192d78017c15caa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Oct 2013 14:11:51 +1100 Subject: [PATCH] HAL_SITL: decrease wind with altitude this prevents JSBSim crash on the runway --- libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index d12811bd47..1405c9df1d 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -490,7 +490,12 @@ void SITL_State::_simulator_output(void) current_pin_value = ((current / 17.0) / 5.0) * 1024; // setup wind control - control.speed = _sitl->wind_speed * 100; + float wind_speed = _sitl->wind_speed * 100; + float altitude = _barometer?_barometer->get_altitude():0; + if (altitude < 60) { + wind_speed *= altitude / 60.0f; + } + control.speed = wind_speed; float direction = _sitl->wind_direction; if (direction < 0) { direction += 360;