From eed6edc3b0e081eceacd474a911a8ed379d51318 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Nov 2022 09:36:25 +1100 Subject: [PATCH] SITL: fixed sign of wind with AHRS_EKF_TYPE=10 and builtin models an alternative to #21929 that doesn't break RealFlight needs testing with sailboats --- libraries/SITL/SIM_Aircraft.cpp | 7 +++++-- libraries/SITL/SIM_Sailboat.cpp | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index ae20f0c989..eed4af0346 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -641,7 +641,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) position += (velocity_ef * delta_time).todouble(); // velocity relative to air mass, in earth frame - velocity_air_ef = velocity_ef + wind_ef; + velocity_air_ef = velocity_ef - wind_ef; // velocity relative to airmass in body frame velocity_air_bf = dcm.transposed() * velocity_air_ef; @@ -785,6 +785,9 @@ void Aircraft::update_wind(const struct sitl_input &input) sinf(radians(turbulence_azimuth)) * turbulence_horizontal_speed, turbulence_vertical_speed); } + + // the AHRS wants wind with opposite sense + wind_ef = -wind_ef; } /* @@ -937,7 +940,7 @@ void Aircraft::extrapolate_sensors(float delta_time) // new velocity and position vectors velocity_ef += accel_earth * delta_time; position += (velocity_ef * delta_time).todouble(); - velocity_air_ef = velocity_ef + wind_ef; + velocity_air_ef = velocity_ef - wind_ef; velocity_air_bf = dcm.transposed() * velocity_air_ef; } diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp index 945b53606c..7d3dba6745 100644 --- a/libraries/SITL/SIM_Sailboat.cpp +++ b/libraries/SITL/SIM_Sailboat.cpp @@ -184,7 +184,7 @@ void Sailboat::update(const struct sitl_input &input) // calculate apparent wind in earth-frame (this is the direction the wind is coming from) // Note than the SITL wind direction is defined as the direction the wind is travelling to // This is accounted for in these calculations - Vector3f wind_apparent_ef = wind_ef + velocity_ef; + Vector3f wind_apparent_ef = velocity_ef - wind_ef; const float wind_apparent_dir_ef = degrees(atan2f(wind_apparent_ef.y, wind_apparent_ef.x)); const float wind_apparent_speed = safe_sqrt(sq(wind_apparent_ef.x)+sq(wind_apparent_ef.y));