From 16cf77da38f951ecdfef65eb71ae67b967c38de3 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 3 Jul 2024 18:36:21 +0200 Subject: [PATCH] only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning and increase horizontal velocity variance to allow velocity states to move towards solution that is aligned with the newly set wind Signed-off-by: RomanBapst --- src/modules/ekf2/EKF/wind.cpp | 5 +++++ src/modules/ekf2/EKF2.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp index a0b814988a..c1aa518047 100644 --- a/src/modules/ekf2/EKF/wind.cpp +++ b/src/modules/ekf2/EKF/wind.cpp @@ -60,6 +60,11 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, _information_events.flags.reset_wind_to_ext_obs = true; resetWindTo(wind, wind_var); + + // reset the horizontal velocity variances to allow the velocity states to be pulled towards + // a solution that is aligned with the newly set wind estimates + static constexpr float hor_vel_var = 25.0f; + P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var); } void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 123b5e37a4..24b92a1ad4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -536,8 +536,10 @@ void EKF2::Run() } if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) { - _ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2, - vehicle_command.param4); + if (_ekf.control_status_flags().wind_dead_reckoning) { + _ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2, + vehicle_command.param4); + } } } }