diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
index aa1195ca2f..bbd26e8125 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
@@ -692,7 +692,8 @@ 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()) {
@@ -700,7 +701,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
* @description Local position estimate valid but has low accuracy. Warn user.
*
*
- * This check can be configured via COM_POS_LOW_EPH parameter.
+ * This check can be configured via COM_POS_LOW_EPH and COM_POS_LOW_ACT parameters.
*
*/
events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info},
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
index d5cb363e11..dd96769ce0 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
@@ -119,6 +119,7 @@ private:
(ParamFloat) _param_com_pos_fs_eph,
(ParamFloat) _param_com_vel_fs_evh,
(ParamInt) _param_com_pos_fs_delay,
- (ParamFloat) _param_com_low_eph
+ (ParamFloat) _param_com_low_eph,
+ (ParamInt) _param_com_pos_low_act
)
};
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index cae2999990..0d3cf5f25f 100644
--- a/src/modules/commander/commander_params.c
+++ b/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
*
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
index 534d62807f..3ba1131d97 100644
--- a/src/modules/commander/failsafe/failsafe.cpp
+++ b/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 ||
diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h
index 2ebb962cb1..7865c207db 100644
--- a/src/modules/commander/failsafe/failsafe.h
+++ b/src/modules/commander/failsafe/failsafe.h
@@ -141,6 +141,15 @@ private:
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 @@ private:
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 @@ private:
(ParamInt) _param_com_obl_rc_act,
(ParamInt) _param_com_qc_act,
(ParamInt) _param_com_wind_max_act,
- (ParamInt) _param_com_fltt_low_act
+ (ParamInt) _param_com_fltt_low_act,
+ (ParamInt) _param_com_pos_low_act
);
};