From 174a031a0335e0623b77e92bf18ab76dcd7df8e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Jul 2022 11:45:27 +1000 Subject: [PATCH] SITL: fill in wind from flightaxis --- libraries/SITL/SIM_Aircraft.cpp | 2 ++ libraries/SITL/SIM_FlightAxis.cpp | 4 ++-- libraries/SITL/SITL.h | 3 +++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 2f8464448a..2245e50d97 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -399,6 +399,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.wind_vane_apparent.direction = wind_vane_apparent.direction; fdm.wind_vane_apparent.speed = wind_vane_apparent.speed; + fdm.wind_ef = wind_ef; + if (is_smoothed) { fdm.xAccel = smoothing.accel_body.x; fdm.yAccel = smoothing.accel_body.y; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 5e34d91fe6..20a03d7bac 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -489,8 +489,8 @@ void FlightAxis::update(const struct sitl_input &input) can't get that from m_airspeed_MPS, so instead we calculate it from wind vector and ground speed */ - Vector3f m_wind_ef(-state.m_windY_MPS,-state.m_windX_MPS,-state.m_windZ_MPS); - Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef; + wind_ef = Vector3f(state.m_windY_MPS,state.m_windX_MPS,state.m_windZ_MPS); + Vector3f airspeed_3d_ef = velocity_ef - wind_ef; Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef); if (last_imu_rotation != ROTATION_NONE) { diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index ec210a99d3..5cf313c717 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -86,6 +86,9 @@ struct sitl_fdm { } wind_vane_apparent; bool is_lock_step_scheduled; + + // earthframe wind, from backends that know it + Vector3f wind_ef; }; // number of rc output channels