From 66aad7a6a59f148ff3ea76f4694ddd8a104c8a55 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:09:33 -0400 Subject: [PATCH] ekf2: GNSS velocity control should own vertical velocity reset if height faiing - GNSS height control using the velocity sample directly is ignoring potential position offsets --- .../EKF/aid_sources/gnss/gnss_height_control.cpp | 9 --------- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 16 +++++++++++----- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index ff04cc3b3977..c141d415eec5 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -108,15 +108,6 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + measurement); - // reset vertical velocity - if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast(GnssCtrl::VEL))) { - // use 1.5 as a typical ratio of vacc/hacc - resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise))); - - } else { - resetVerticalVelocityToZero(); - } - aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index d5bca4f46ac3..b18fa37ed981 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -132,17 +132,23 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } if (do_vel_pos_reset) { - ECL_WARN("GPS fusion timeout, resetting velocity / position"); + ECL_WARN("GPS fusion timeout, resetting"); + } - if (gnss_vel_enabled) { + if (gnss_vel_enabled) { + if (do_vel_pos_reset) { resetVelocityToGnss(_aid_src_gnss_vel); - } - if (gnss_pos_enabled) { - resetHorizontalPositionToGnss(_aid_src_gnss_pos); + } else if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]); } } + if (gnss_pos_enabled && do_vel_pos_reset) { + resetHorizontalPositionToGnss(_aid_src_gnss_pos); + } + } else { stopGpsFusion(); }