From 130fefb1e7f3fc3d9948cd49adf7ac19ca28fc9d Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 21 Aug 2024 16:32:29 +0200 Subject: [PATCH] ekf2: initialize origin from corrent position when possible --- .../aid_sources/aux_global_position/aux_global_position.cpp | 3 ++- src/modules/ekf2/EKF/ekf.cpp | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 6efd4fabe6..dc8f318c97 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } else { // Try to initialize using measurement - if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) { + if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, + sample.epv)) { ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 9f6a0ab40b..a81cc39d62 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -280,8 +280,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl uint64_t timestamp_observation) { if (!_pos_ref.isInitialized()) { - ECL_WARN("unable to reset global position, position reference not initialized"); - return false; + return setLatLonOriginFromCurrentPos(lat_deg, lon_deg, accuracy); } Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);