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.

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);