From d0b9c3a094efe6f6d8a367cbbcdd477230ff34a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 May 2024 10:45:22 +1000 Subject: [PATCH 1/2] SITL: added SIM_WIND_TC time constant for wind changes --- libraries/SITL/SITL.cpp | 8 ++++++++ libraries/SITL/SITL.h | 1 + 2 files changed, 9 insertions(+) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 7ca40de3d9d5c..98a5afa034c0b 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -81,6 +81,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Units: deg // @User: Advanced AP_GROUPINFO("WIND_DIR", 10, SIM, wind_direction, 180), + // @Param: WIND_TURB // @DisplayName: Simulated Wind variation // @Description: Allows you to emulate random wind variations in sim @@ -88,6 +89,13 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @User: Advanced AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0), + // @Param: WIND_TC + // @DisplayName: Wind variation time constant + // @Description: this controls the time over which wind changes take effect + // @Units: s + // @User: Advanced + AP_GROUPINFO("WIND_TC", 12, SIM, wind_change_tc, 5), + // @Group: SERVO_ // @Path: ./ServoModel.cpp AP_SUBGROUPINFO(servo, "SERVO_", 16, SIM, ServoParams), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 4d8beb2eddd9b..718070bbc44b9 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -349,6 +349,7 @@ class SIM { AP_Float wind_direction; AP_Float wind_turbulance; AP_Float wind_dir_z; + AP_Float wind_change_tc; AP_Int8 wind_type; // enum WindLimitType AP_Float wind_type_alt; AP_Float wind_type_coef; From f58a358fea0fd92bd7c4502b1cc6e616008c62a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Jul 2022 16:39:04 +1000 Subject: [PATCH 2/2] HAL_SITL: implement wind rate of change --- libraries/AP_HAL_SITL/SITL_State.cpp | 18 +++++++++++++++--- libraries/AP_HAL_SITL/SITL_State.h | 1 + 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 8bf0abf8a5763..85d3295445ae5 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -327,9 +327,21 @@ void SITL_State::_simulator_servos(struct sitl_input &input) wind_start_delay_micros = now; } else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) { // The EKF does not like step inputs so this LPF keeps it happy. - wind_speed = _sitl->wind_speed_active = (0.95f*_sitl->wind_speed_active) + (0.05f*_sitl->wind_speed); - wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction); - wind_dir_z = _sitl->wind_dir_z_active = (0.95f*_sitl->wind_dir_z_active) + (0.05f*_sitl->wind_dir_z); + uint32_t dt_us = now - last_wind_update_us; + if (dt_us > 1000) { + last_wind_update_us = now; + // slew wind based on the configured time constant + const float dt = dt_us * 1.0e-6; + const float tc = MAX(_sitl->wind_change_tc, 0.1); + const float alpha = calc_lowpass_alpha_dt(dt, 1.0/tc); + _sitl->wind_speed_active += (_sitl->wind_speed - _sitl->wind_speed_active) * alpha; + _sitl->wind_direction_active += (wrap_180(_sitl->wind_direction - _sitl->wind_direction_active)) * alpha; + _sitl->wind_dir_z_active += (_sitl->wind_dir_z - _sitl->wind_dir_z_active) * alpha; + _sitl->wind_direction_active = wrap_180(_sitl->wind_direction_active); + } + wind_speed = _sitl->wind_speed_active; + wind_direction = _sitl->wind_direction_active; + wind_dir_z = _sitl->wind_dir_z_active; // pass wind into simulators using different wind types via param SIM_WIND_T*. switch (_sitl->wind_type) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 91f0a85739c45..1a0074f14a882 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -107,6 +107,7 @@ class HALSITL::SITL_State : public SITL_State_Common { uint32_t time_delta_wind; uint32_t delayed_time_wind; uint32_t wind_start_delay_micros; + uint32_t last_wind_update_us; // simulated GPS devices SITL::GPS *gps[2]; // constrained by # of parameter sets