From e7f3716e8c47f04109d837fdeef1c9b0016ff141 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 23 May 2015 11:04:13 +1000 Subject: [PATCH] HAL_SITL: pass in wind speed to C++ simulators --- libraries/AP_HAL_SITL/SITL_State.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 05c3e422e7..56d1eb61a2 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -382,6 +382,18 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input) _apply_servo_filter(deltat); + // pass wind into simulators, using a wind gradient below 60m + float altitude = _barometer?_barometer->get_altitude():0; + float wind_speed = _sitl->wind_speed; + if (altitude < 0) { + altitude = 0; + } + if (altitude < 60) { + wind_speed *= altitude / 60; + } + input.wind.speed = wind_speed; + input.wind.direction = _sitl->wind_direction; + for (i=0; i<11; i++) { if (pwm_output[i] == 0xFFFF) { input.servos[i] = 0;