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