Commander: add local_position_accuracy_low flag, incl. warning and RTL

Set this flag to true if local position is valid but accuracy low, such that
the operator can be warned before system switches to position-failure failsafe.
Additionally, switch to RTL if currently in Mission or Loiter to try to reach home
or fly out of GNSS-denied area.

Set low accuracy threshold to 50m by default for FW and VTOL.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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 <param>COM_POS_LOW_EPH</param> 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)

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<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
-					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay
+					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
+					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph
 				       )
 };

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);

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