commander: unify offboard timeouts

The implementation before this change had two timeouts, a hard-coded
timeout of 0.5 seconds as well as a by param configurable timeout with
certain failsafe actions set.

This change aims to fix two problems:
1. The hard-coded offboard timeout can be triggered easily with sped up
   lockstep simulation. Since i t is hard-coded it can't be adapted to
   the speed factor.
2. The offboard signal can time out but no action will be taken just
   yet. This means we end up in an in-between stage where no warning or
   failsafe action has happened yet, even though certain flags are set
   to a timeout state.

This patch aims to fix this by unifying the two timeouts to the existing
configurable param. The convoluted double timeout logic is replaced by a
simple hysteresis.

For anyone that has previously not changed the default timeout param (0),
the param will now be changed to 0.5 seconds which reflects the
previously hardcoded time. For anyone with a specific timeout
configured, the behaviour should remain the same.

Also, going forward, timeouts lower than 0.5 seconds should be possible.

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -135,7 +135,6 @@ then
 	param set COM_DISARM_LAND 0.5
 	param set COM_OBL_ACT 2
 	param set COM_OBL_RC_ACT 0
-	param set COM_OF_LOSS_T 5
 	param set COM_RC_IN_MODE 1
 
 	param set EKF2_AID_MASK 1
@@ -199,11 +198,11 @@ if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
 	echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
 	param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
 
-	COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1" | bc)
+	COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
 	echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
 	param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
 
-	COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
+	COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
 	echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
 	param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
 fi

msg/vehicle_status_flags.msg

@@ -26,7 +26,6 @@ bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in
 bool offboard_control_signal_found_once
 bool offboard_control_signal_lost
 bool offboard_control_set_by_command                # true if the offboard mode was set by a mavlink command and should not be overridden by RC
-bool offboard_control_loss_timeout                # true if offboard is lost for a certain amount of time
 
 bool rc_signal_found_once
 bool rc_input_blocked                                # set if RC input should be ignored temporarily

src/modules/commander/Commander.cpp

@@ -1369,6 +1369,8 @@ Commander::run()
 				}
 			}
 
+			_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get());
+
 			param_init_forced = false;
 		}
 
@@ -3993,38 +3995,16 @@ Commander::offboard_control_update()
 		}
 	}
 
-	if (offboard_control_mode.timestamp != 0 &&
-	    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
-		if (status_flags.offboard_control_signal_lost) {
-			status_flags.offboard_control_signal_lost = false;
-			status_flags.offboard_control_loss_timeout = false;
-			_status_changed = true;
-		}
+	_offboard_available.set_state_and_update(
+		hrt_elapsed_time(&offboard_control_mode.timestamp) < _param_com_of_loss_t.get() * 1e6f,
+		hrt_absolute_time());
 
-	} else {
-		if (!status_flags.offboard_control_signal_lost) {
-			status_flags.offboard_control_signal_lost = true;
-			_status_changed = true;
-		}
+	const bool offboard_lost = !_offboard_available.get_state();
 
-		/* check timer if offboard was there but now lost */
-		if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) {
-			if (_param_com_of_loss_t.get() < FLT_EPSILON) {
-				/* execute loss action immediately */
-				status_flags.offboard_control_loss_timeout = true;
-
-			} else {
-				/* wait for timeout if set */
-				status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
-						OFFBOARD_TIMEOUT + _param_com_of_loss_t.get() * 1e6f < hrt_absolute_time();
-			}
-
-			if (status_flags.offboard_control_loss_timeout) {
-				_status_changed = true;
-			}
-		}
+	if (status_flags.offboard_control_signal_lost != offboard_lost) {
+		status_flags.offboard_control_signal_lost = offboard_lost;
+		_status_changed = true;
 	}
-
 }
 
 void Commander::esc_status_check(const esc_status_s &esc_status)

src/modules/commander/Commander.hpp

@@ -276,7 +276,6 @@ private:
 
 	static constexpr float STICK_ON_OFF_LIMIT{0.9f};
 
-	static constexpr uint64_t OFFBOARD_TIMEOUT{500_ms};
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
 	static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
@@ -330,6 +329,7 @@ private:
 
 	Hysteresis	_auto_disarm_landed{false};
 	Hysteresis	_auto_disarm_killed{false};
+	Hysteresis	_offboard_available{false};
 
 	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
 

src/modules/commander/commander_params.c

@@ -330,9 +330,9 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @unit s
  * @min 0
  * @max 60
- * @increment 1
+ * @increment 0.01
  */
-PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f);
+PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.5f);
 
 /**
  * Set offboard loss failsafe mode

src/modules/commander/state_machine_helper.cpp

@@ -684,8 +684,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 
 	case commander_state_s::MAIN_STATE_OFFBOARD:
 
-		/* require offboard control, otherwise stay where you are */
-		if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_loss_timeout) {
+		if (status_flags.offboard_control_signal_lost) {
 			if (status->rc_signal_lost) {
 				// Offboard and RC are lost
 				enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);