Commander: enable failsafe delay for position and mission mode

Instead of directly doing the link loss reaction which by default is RTL a delay
can be configured such that the drone first switches to hold and waits
for the link to be regained.

msg/vehicle_status.msg

@@ -63,6 +63,7 @@ uint64 nav_state_timestamp              # time when current nav_state activated
 uint8 arming_state			# current arming state
 uint8 hil_state				# current hil state
 bool failsafe				# true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
+uint64 failsafe_timestamp               # time when failsafe was activated
 
 uint8 system_type			# system type, contains mavlink MAV_TYPE
 uint8 system_id			# system id, contains MAVLink's system ID field

src/modules/commander/Commander.cpp

@@ -2545,7 +2545,8 @@ Commander::run()
 						       (link_loss_actions_t)_param_nav_rcl_act.get(),
 						       (offboard_loss_actions_t)_param_com_obl_act.get(),
 						       (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(),
-						       (position_nav_loss_actions_t)_param_com_posctl_navl.get());
+						       (position_nav_loss_actions_t)_param_com_posctl_navl.get(),
+						       _param_com_ll_delay.get());
 
 		if (nav_state_changed) {
 			_status.nav_state_timestamp = hrt_absolute_time();

src/modules/commander/Commander.hpp

@@ -189,6 +189,7 @@ private:
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
 		(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
+		(ParamFloat<px4::params::COM_LL_DELAY>) _param_com_ll_delay,
 
 		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
 		(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,

src/modules/commander/commander_params.c

@@ -968,3 +968,18 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);
 * @value 1 Enabled
 */
 PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
+
+/**
+ * Delay between link loss and configured reaction in Position and Mission mode
+ *
+ * A non-zero, positive value makes the failsafe reaction first stop the vehicle and wait
+ * before proceeding with the configured failsafe reaction NAV_RCL_ACT.
+ * A zero or negative value disables the delay.
+ *
+ * @group Commander
+ * @unit s
+ * @min -1.0
+ * @max 60.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(COM_LL_DELAY, 15.0f);

src/modules/commander/state_machine_helper.cpp

@@ -45,10 +45,13 @@
 #include <uORB/topics/vehicle_status.h>
 #include <systemlib/mavlink_log.h>
 #include <drivers/drv_hrt.h>
+#include <float.h>
 
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
+using namespace time_literals;
+
 static constexpr const char reason_no_rc[] = "No manual control stick input";
 static constexpr const char reason_no_offboard[] = "no offboard";
 static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offboard";
@@ -385,6 +388,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
 {
 	if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		status->failsafe_timestamp = hrt_absolute_time();
 		mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason);
 	}
 
@@ -399,7 +403,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
 		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
 		   const offboard_loss_rc_actions_t offb_loss_rc_act,
-		   const position_nav_loss_actions_t posctl_nav_loss_act)
+		   const position_nav_loss_actions_t posctl_nav_loss_act,
+		   const float param_com_ll_delay)
 {
 	const navigation_state_t nav_state_old = status->nav_state;
 
@@ -465,7 +470,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 			if (rc_lost && is_armed) {
 				enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
 
-				set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
+				if (hrt_elapsed_time(&status->failsafe_timestamp) > (param_com_ll_delay * 1_s)) {
+					/* only start reaction after configured delay */
+					set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
+
+				} else {
+					/* wait in hold mode if delay is configured */
+					set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_LOITER);
+				}
 
 				/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
 				/* A local position estimate is enough for POSCTL for multirotors,
@@ -508,7 +520,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 			 * check for datalink lost: this should always trigger RTL */
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 
-			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
+			if (hrt_elapsed_time(&status->failsafe_timestamp) > (param_com_ll_delay * 1_s)) {
+				/* only start reaction after configured delay */
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
+
+			} else {
+				/* wait in hold mode if delay is configured */
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_LOITER);
+			}
 
 		} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
 			   && mission_finished && is_armed) {

src/modules/commander/state_machine_helper.h

@@ -131,7 +131,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
 		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
 		   const offboard_loss_rc_actions_t offb_loss_rc_act,
-		   const position_nav_loss_actions_t posctl_nav_loss_act);
+		   const position_nav_loss_actions_t posctl_nav_loss_act,
+		   const float param_com_ll_delay);
 
 /*
  * Checks the validty of position data against the requirements of the current navigation