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>

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
 

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
 

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)

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