Commander: allow disabling RC loss failsafe for mission, hold, offboard independently

src/modules/commander/Commander.cpp

@@ -2575,7 +2575,8 @@ Commander::run()
 						       static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
 						       static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
 						       static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
-						       _param_com_rcl_act_t.get());
+						       _param_com_rcl_act_t.get(),
+						       _param_com_rcl_except.get());
 
 		if (nav_state_changed) {
 			_status.nav_state_timestamp = hrt_absolute_time();

src/modules/commander/Commander.hpp

@@ -198,6 +198,7 @@ private:
 
 		(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
 		(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
+		(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
 
 		(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
 		(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,

src/modules/commander/commander_params.c

@@ -850,6 +850,8 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
  * @value 3 Land mode
  * @value 5 Terminate
  * @value 6 Lockdown
+ * @min 0
+ * @max 6
  *
  * @group Commander
  */
@@ -862,17 +864,32 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
  * by setting the COM_RC_IN_MODE param it will not be triggered.
  *
- * @value 0 Disabled
  * @value 1 Hold mode
  * @value 2 Return mode
  * @value 3 Land mode
  * @value 5 Terminate
  * @value 6 Lockdown
+ * @min 1
+ * @max 6
  *
  * @group Commander
  */
 PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
 
+/**
+ * RC loss exceptions
+ *
+ * Specify modes in which RC loss is ignored and the failsafe action not triggered.
+ *
+ * @min 0
+ * @max 31
+ * @bit 0 Mission
+ * @bit 1 Hold
+ * @bit 2 Offboard
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
+
 /**
  * Flag to enable obstacle avoidance.
  *

src/modules/commander/state_machine_helper.cpp

@@ -436,15 +436,13 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		   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 float param_com_rcl_act_t)
+		   const float param_com_rcl_act_t, const int param_com_rcl_except)
 {
 	const navigation_state_t nav_state_old = status.nav_state;
 
-	const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
+	const bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-	const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
+	const bool data_link_loss_act_configured = (data_link_loss_act > link_loss_actions_t::DISABLED);
-	const bool rc_lost = rc_loss_act_configured && (status.rc_signal_lost);
 
-	bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 	bool old_failsafe = status.failsafe;
 	status.failsafe = false;
 
@@ -453,17 +451,16 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	reset_link_loss_globals(armed, old_failsafe, data_link_loss_act);
 	reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act);
 
-	/* evaluate main state to decide in normal (non-failsafe) mode */
+	// Failsafe decision logic for every normal non-failsafe mode
 	switch (internal_state.main_state) {
 	case commander_state_s::MAIN_STATE_ACRO:
 	case commander_state_s::MAIN_STATE_MANUAL:
 	case commander_state_s::MAIN_STATE_STAB:
 	case commander_state_s::MAIN_STATE_ALTCTL:
 
-		/* require RC for all manual modes */
+		// Require RC for all manual modes
-		if (rc_lost && is_armed) {
+		if (status.rc_signal_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, param_com_rcl_act_t);
 
 		} else {
@@ -496,7 +493,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 			const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed;
 
-			if (rc_lost && is_armed) {
+			if (status.rc_signal_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, param_com_rcl_act_t);
 
@@ -539,27 +536,27 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			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, 0);
 
-		} else if (status.rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once
+		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
-			   && is_armed && !landed) {
+			   && status_flags.rc_signal_found_once && is_armed && !landed) {
-			// RC link lost, rc loss reaction configured, RC was used before -> RC loss reaction after delay
+			// RC link lost, rc loss not disabled in mission, RC was used before -> RC loss reaction after delay
 			// Safety pilot expects to be able to take over by RC in case anything unexpected happens
 			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, param_com_rcl_act_t);
 
-		} else if (status.rc_signal_lost && rc_loss_act_configured
+		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
 			   && status.data_link_lost && !data_link_loss_act_configured
 			   && is_armed && !landed) {
 			// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
 			// Lost all communication, by default it's considered unsafe to continue the mission
-			// Note this case is reached after the previous one when flying mission completely without RC
+			// This is only reached when flying mission completely without RC (it was not present since boot)
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
 
-		} else if (status.rc_signal_lost && !rc_loss_act_configured
+		} else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
 			   && status.data_link_lost && !data_link_loss_act_configured
 			   && is_armed && !landed
 			   && mission_finished) {
-			// All links lost, all link loss reactions disabled -> return after mission
+			// All links lost, all link loss reactions disabled -> return after mission finished
 			// Pilot disabled all reactions, finish mission but then return to avoid lost vehicle
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 			set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);
@@ -580,25 +577,35 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
-			/* also go into failsafe if just datalink is lost, and we're actually in air */
+			// Data link lost, data link loss reaction configured -> do configured reaction
+			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, 0);
 
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
+		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
-
+			   && status_flags.rc_signal_found_once && is_armed && !landed) {
-		} else if (rc_lost && !data_link_loss_act_configured && status.data_link_lost && is_armed) {
+			// RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay
-			/* go into failsafe if RC is lost and datalink is lost and datalink loss is not set up */
+			// Safety pilot expects to be able to take over by RC in case anything unexpected happens
 			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, param_com_rcl_act_t);
 
+		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
+			   && status.data_link_lost && !data_link_loss_act_configured
+			   && is_armed && !landed) {
+			// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
+			// Lost all communication, by default it's considered unsafe to continue the mission
+			// This is only reached when flying mission completely without RC (it was not present since boot)
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
 			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
 
-		} else if (status.rc_signal_lost) {
+		} else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
-			/* don't bother if RC is lost if datalink is connected */
+			   && status.data_link_lost && !data_link_loss_act_configured
-
+			   && is_armed && !landed) {
-			/* this mode is ok, we don't need RC for LOITERing */
+			// All links lost, all link loss reactions disabled -> return
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+			// Pilot disabled all reactions, return to avoid lost vehicle
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);
 
 		} else {
-			/* everything is perfect */
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		}
 
@@ -661,7 +668,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			// is not possible and therefore the internal_state needs to be adjusted.
 			internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
 
-		} else if (rc_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) {
+		} else if (status.rc_signal_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) {
 			// Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
 
@@ -729,7 +736,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_OFFBOARD:
 
 		if (status_flags.offboard_control_signal_lost) {
-			if (status.rc_signal_lost) {
+			if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) {
 				// Offboard and RC are lost
 				enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);
 				set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act);

src/modules/commander/state_machine_helper.h

@@ -115,6 +115,12 @@ enum class arm_disarm_reason_t {
 	UNIT_TEST = 13
 };
 
+enum RCLossExceptionBits {
+	RCL_EXCEPT_MISSION = (1 << 0),
+	RCL_EXCEPT_HOLD = (1 << 1),
+	RCL_EXCEPT_OFFBOARD = (1 << 2)
+};
+
 transition_result_t
 arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
 			actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
@@ -133,7 +139,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		   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 float param_com_rcl_act_t);
+		   const float param_com_rcl_act_t, const int param_com_rcl_except);
 
 /*
  * Checks the validty of position data against the requirements of the current navigation