diff --git a/src/modules/ekf2/EKF/aux_global_position.cpp b/src/modules/ekf2/EKF/aux_global_position.cpp index 269ba29e44..f64a990bae 100644 --- a/src/modules/ekf2/EKF/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aux_global_position.cpp @@ -56,6 +56,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed sample.altitude_amsl = aux_global_position.alt; sample.eph = aux_global_position.eph; sample.epv = aux_global_position.epv; + sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter; _aux_global_position_buffer.push(sample); @@ -103,12 +104,14 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (ekf.global_origin_valid()) { ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; } else { // Try to initialize using measurement if (ekf.setEkfGlobalOrigin(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; } } @@ -119,9 +122,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (continuing_conditions) { ekf.fuseHorizontalPosition(aid_src); - if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)) { + if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) + || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); aid_src.time_last_fuse = imu_delayed.time_us; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; } } else { diff --git a/src/modules/ekf2/EKF/aux_global_position.hpp b/src/modules/ekf2/EKF/aux_global_position.hpp index 072aacac55..4246477350 100644 --- a/src/modules/ekf2/EKF/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aux_global_position.hpp @@ -87,6 +87,7 @@ private: float altitude_amsl{}; float eph{}; float epv{}; + uint8_t lat_lon_reset_counter{}; }; RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate @@ -106,6 +107,11 @@ private: State _state{State::stopped}; #if defined(MODULE_NAME) + struct reset_counters_s { + uint8_t lat_lon{}; + }; + reset_counters_s _reset_counters{}; + uORB::PublicationMulti _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)}; uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};