diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
index 44b5fa91c9..9bb33f2bd2 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
@@ -117,7 +117,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
// set mode requirements
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
- reporter.failsafeFlags());
+ reporter.failsafeFlags(), reporter);
lowPositionAccuracy(context, reporter, lpos);
@@ -731,7 +731,8 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
bool pre_flt_fail_innov_vel_horiz,
- const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags)
+ const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags,
+ Report &reporter)
{
// The following flags correspond to mode requirements, and are reported in the corresponding mode checks
vehicle_global_position_s gpos;
@@ -772,6 +773,47 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
!checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
+ // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
+ const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe
+ const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
+
+ estimator_status_flags_s estimator_status_flags;
+
+ if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
+
+ // only do the following if the estimator status flags are recent (less than 5 seconds old)
+ if (now - estimator_status_flags.timestamp < 5_s) {
+ const bool dead_reckoning = estimator_status_flags.cs_inertial_dead_reckoning
+ || estimator_status_flags.cs_wind_dead_reckoning;
+
+ if (!failsafe_flags.global_position_invalid
+ && !_nav_failure_imminent_warned
+ && gpos.eph > gpos_critical_warning_thrld
+ && dead_reckoning) {
+ /* EVENT
+ * @description
+ * Switch to manual mode recommended.
+ *
+ *
+ * This warning is triggered when the position error estimate is 90% of (or only 10m below) COM_POS_FS_EPH parameter.
+ *
+ */
+ events::send(events::ID("check_estimator_position_failure_imminent"), {events::Log::Error, events::LogInternal::Info},
+ "Estimated position error is approaching the failsafe threshold");
+
+ if (reporter.mavlink_log_pub()) {
+ mavlink_log_critical(reporter.mavlink_log_pub(),
+ "Estimated position error is approaching the failsafe threshold\t");
+ }
+
+ _nav_failure_imminent_warned = true;
+
+ } else if (!dead_reckoning) {
+ _nav_failure_imminent_warned = false;
+ }
+ }
+ }
+
failsafe_flags.local_position_invalid =
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp,
_last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
index 4dd45cd70e..f5b8073524 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
@@ -69,7 +69,7 @@ private:
void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
- failsafe_flags_s &failsafe_flags);
+ failsafe_flags_s &failsafe_flags, Report &reporter);
bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
const float required_accuracy,
@@ -103,6 +103,8 @@ private:
bool _gps_was_fused{false};
+ bool _nav_failure_imminent_warned{false};
+
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt) _param_sys_mc_est_group,
(ParamInt) _param_sens_imu_mode,