From cd2170deea5d7b2bb387ddc3ba7a7c3abd58b133 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 26 Aug 2024 11:53:40 +0200 Subject: [PATCH] ekf2-origin: backcompute based on lpos validity --- src/modules/ekf2/EKF/ekf_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 6e3bdf3bab..f33cbcd5bc 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -110,7 +110,7 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f double current_lon = static_cast(NAN); // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && isHorizontalAidingActive()) { + if (_pos_ref.isInitialized() && local_position_is_valid()) { _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); current_pos_available = true; } @@ -146,7 +146,7 @@ bool Ekf::setAltOrigin(const float altitude, const float epv) _gpos_origin_epv = epv; } - if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) { + if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) { // determine current z const float z_prev = _state.pos(2); const float current_alt = -z_prev + gps_alt_ref_prev; @@ -187,7 +187,7 @@ bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double long _pos_ref.initReference(latitude, longitude, _time_delayed_us); // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (isHorizontalAidingActive()) { + if (local_position_is_valid()) { double est_lat; double est_lon; _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);