Commander: put action for low position estimation accuracy into param…

…eter

Signed-off-by: Silvan Fuhrer <[email protected]>

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -692,15 +692,16 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
 	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) {
+	if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy
+	    && _param_com_pos_low_act.get()) {
 
 		// only report if armed
 		if (context.isArmed()) {
 			/* EVENT
 			 * @description Local position estimate valid but has low accuracy. Warn user.
 			 *
 			 * <profile name="dev">
-			 * This check can be configured via <param>COM_POS_LOW_EPH</param> parameter.
+			 * This check can be configured via <param>COM_POS_LOW_EPH</param> and <param>COM_POS_LOW_ACT</param> parameters.
 			 * </profile>
 			 */
 			events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info},

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -119,6 +119,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
 					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
-					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph
+					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
+					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
 				       )
 };

src/modules/commander/commander_params.c

@@ -928,13 +928,11 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
 PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0);
 
 /**
- * EPH threshold for RTL
+ * Low position accuracy failsafe threshold
  *
- * 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.
+ * This triggers the action specified in COM_POS_LOW_ACT if the estimated position accuracy is below this threshold.
+ * Local position has to be still declared valid, which requires some kind of velocity aiding or large dead-reckoning time (EKF2_NOAID_TOUT),
+ * and a high failsafe threshold (COM_POS_FS_EPH).
  *
  * Set to -1 to disable.
  *
@@ -945,6 +943,26 @@ PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0);
  */
 PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
 
+/**
+ * Low position accuracy action
+ *
+ * Action the system takes when the estimated position has an accuracy below the specified threshold.
+ * See COM_POS_LOW_EPH to set the failsafe threshold.
+ * The failsafe action is only executed if the vehicle is in auto mission or auto loiter mode,
+ * otherwise it is only a warning.
+ *
+ * @group Commander
+ *
+ * @value 0 None
+ * @value 1 Warning
+ * @value 2 Hold
+ * @value 3 Return
+ * @value 4 Terminate
+ * @value 5 Land
+ * @increment 1
+ */
+PARAM_DEFINE_INT32(COM_POS_LOW_ACT, 3);
+
 /**
  * Flag to allow arming
  *

src/modules/commander/failsafe/failsafe.cpp

@@ -368,6 +368,49 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 	return options;
 }
 
+FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
+{
+	ActionOptions options{};
+	options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again
+
+	switch (command_after_pos_low_failsafe(param_value)) {
+	case command_after_pos_low_failsafe::None:
+		options.action = Action::None;
+		break;
+
+	case command_after_pos_low_failsafe::Warning:
+		options.action = Action::Warn;
+		break;
+
+	case command_after_pos_low_failsafe::Hold_mode:
+		options.action = Action::Hold;
+		options.clear_condition = ClearCondition::WhenConditionClears;
+		break;
+
+	case command_after_pos_low_failsafe::Return_mode:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::WhenConditionClears;
+		break;
+
+	case command_after_pos_low_failsafe::Terminate:
+		options.allow_user_takeover = UserTakeoverAllowed::Never;
+		options.action = Action::Terminate;
+		options.clear_condition = ClearCondition::Never;
+		break;
+
+	case command_after_pos_low_failsafe::Land_mode:
+		options.action = Action::Land;
+		options.clear_condition = ClearCondition::WhenConditionClears;
+		break;
+
+	default:
+		options.action = Action::Warn;
+		break;
+	}
+
+	return options;
+}
+
 FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
 {
 	ActionOptions options{};
@@ -469,10 +512,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		       ActionOptions(fromHighWindLimitActParam(_param_com_wind_max_act.get()).cannotBeDeferred()));
 	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());
 
-	// trigger RTL if low position accurancy is detected
+	// trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter)
 	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, local_position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
 	}
 
 	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||

src/modules/commander/failsafe/failsafe.h

@@ -141,6 +141,15 @@ class Failsafe : public FailsafeBase
 		Land_mode = 5
 	};
 
+	enum class command_after_pos_low_failsafe : int32_t {
+		None = 0,
+		Warning = 1,
+		Hold_mode = 2,
+		Return_mode = 3,
+		Terminate = 4,
+		Land_mode = 5
+	};
+
 	enum class command_after_remaining_flight_time_low : int32_t {
 		None = 0,
 		Warning = 1,
@@ -156,6 +165,7 @@ class Failsafe : public FailsafeBase
 	static ActionOptions fromQuadchuteActParam(int param_value);
 	static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
 	static ActionOptions fromHighWindLimitActParam(int param_value);
+	static ActionOptions fromPosLowActParam(int param_value);
 	static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
 
 	const int _caller_id_mode_fallback{genCallerId()};
@@ -191,7 +201,8 @@ class Failsafe : public FailsafeBase
 					(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
 					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
 					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
-					(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act
+					(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
+					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
 				       );
 
 };