diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index e896756107..eafe1cd017 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -19,6 +19,9 @@ param set-default COM_POS_FS_DELAY 5
param set-default COM_POS_FS_EPH 50
param set-default COM_POS_FS_EPV 30
param set-default COM_VEL_FS_EVH 5
+
+param set-default COM_POS_LOW_EPH 50
+
# Disable preflight disarm to not interfere with external launching
param set-default COM_DISARM_PRFLT -1
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
index 2f4cb66bb9..6a0bc29952 100644
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@@ -13,6 +13,8 @@ param set-default MAV_TYPE 22
# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
param set-default COM_POS_FS_EPH 50
+param set-default COM_POS_LOW_EPH 50
+
param set-default MIS_TAKEOFF_ALT 20
param set-default MIS_YAW_TMT 10
diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg
index 8271298f11..cf83ec08cf 100644
--- a/msg/FailsafeFlags.msg
+++ b/msg/FailsafeFlags.msg
@@ -48,6 +48,7 @@ bool mission_failure # Mission failure
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
bool wind_limit_exceeded # Wind limit exceeded
bool flight_time_limit_exceeded # Maximum flight time exceeded
+bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
# Failure detector
bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS)
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
index 9828dcb920..deb424653a 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
@@ -126,6 +126,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
gpsNoLongerValid(context, reporter);
}
+
+ lowPositionAccuracy(context, reporter, lpos);
}
void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
@@ -669,6 +671,32 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
}
}
+void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
+ const vehicle_local_position_s &lpos) const
+{
+ const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
+ && (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
+
+ if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy) {
+
+ // only report if armed
+ if (context.isArmed()) {
+ /* EVENT
+ * @description Local position estimate valid but has low accuracy. Warn user.
+ * This check can be configured via COM_POS_LOW_EPH parameter.
+ */
+ events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info},
+ "Local position estimate has low accuracy");
+
+ if (reporter.mavlink_log_pub()) {
+ mavlink_log_warning(reporter.mavlink_log_pub(), "Local position estimate has low accuracy\t");
+ }
+ }
+ }
+
+ reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
+}
+
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)
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
index f591cf03da..0bd2502fcd 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
@@ -67,6 +67,7 @@ private:
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
void gpsNoLongerValid(const Context &context, Report &reporter) const;
+ 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);
@@ -117,6 +118,7 @@ private:
(ParamFloat) _param_com_pos_fs_eph,
(ParamFloat) _param_com_pos_fs_epv,
(ParamFloat) _param_com_vel_fs_evh,
- (ParamInt) _param_com_pos_fs_delay
+ (ParamInt) _param_com_pos_fs_delay,
+ (ParamFloat) _param_com_low_eph
)
};
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 43b8606c34..5d17953644 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -1088,3 +1088,20 @@ PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
* @unit m/s
*/
PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
+
+/**
+ * EPH threshold for RTL
+ *
+ * Specify the threshold for triggering a warning for low local position accuracy. Additionally triggers
+ * a RTL if currently in Mission or Loiter mode.
+ * Local position has to be still declared valid, which is most of all depending on COM_POS_FS_EPH.
+ * Use this feature on systems with dead-reckoning capabilites (e.g. fixed-wing vehicles with airspeed sensor)
+ * to improve the user notification and failure mitigation when flying in GNSS-denied areas.
+ *
+ * Set to -1 to disable.
+ *
+ * @group Commander
+ * @min -1
+ * @unit m
+ */
+PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
index 69747868f1..4d26276ef2 100644
--- a/src/modules/commander/failsafe/failsafe.cpp
+++ b/src/modules/commander/failsafe/failsafe.cpp
@@ -395,6 +395,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL));
+ // trigger RTL if low position accurancy is detected
+ if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+ state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+ CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
+ }
+
CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
// Battery