From 1103e92884eaf1ad5cf66b8812e61c0d9d3d86b7 Mon Sep 17 00:00:00 2001 From: Nicholas Engle Date: Tue, 30 Jan 2018 08:38:36 -0800 Subject: [PATCH] SITL: Add SIM_WIND_DIR_Z parameter for SITL This controls the vertical pitch of the 3d wind vector, allowing futher control of the wind using systems like dronekit. This change directly effects the calcuation of the wind vector --- libraries/SITL/SIM_Aircraft.cpp | 4 +++- libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SITL.cpp | 1 + libraries/SITL/SITL.h | 2 ++ 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 8ffdf96a44..ec97c7711c 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -565,7 +565,9 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) void Aircraft::update_wind(const struct sitl_input &input) { // wind vector in earth frame - wind_ef = Vector3f(cosf(radians(input.wind.direction)), sinf(radians(input.wind.direction)), 0) * input.wind.speed; + wind_ef = Vector3f(cosf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)), + sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)), + sinf(radians(input.wind.dir_z))) * input.wind.speed; const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98 const float iir_coef = 0.98f; // filtering high frequencies from turbulence diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 6a64231494..c2a0727325 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -45,6 +45,7 @@ public: float speed; // m/s float direction; // degrees 0..360 float turbulence; + float dir_z; //degrees -90..90 } wind; }; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 18b025a355..86fb46d1ec 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -108,6 +108,7 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_GROUPINFO("ARSPD_PITOT", 7, SITL, arspd_fail_pitot_pressure, 0), AP_GROUPINFO("GPS_ALT_OFS", 8, SITL, gps_alt_offset, 0), AP_GROUPINFO("ARSPD_SIGN", 9, SITL, arspd_signflip, 0), + AP_GROUPINFO("WIND_DIR_Z", 10, SITL, wind_dir_z, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index cd954ab450..b3b7b2713d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -128,10 +128,12 @@ public: // wind control float wind_speed_active; float wind_direction_active; + float wind_dir_z_active; AP_Float wind_speed; AP_Float wind_direction; AP_Float wind_turbulance; AP_Float gps_drift_alt; + AP_Float wind_dir_z; AP_Int16 baro_delay; // barometer data delay in ms AP_Int16 mag_delay; // magnetometer data delay in ms