From 874c6f385b2ab4ee23e33d13cbe42584fff8f399 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Mar 2020 18:24:11 +0100 Subject: [PATCH] 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 | 5 +-- msg/vehicle_status_flags.msg | 1 - src/modules/commander/Commander.cpp | 38 +++++-------------- src/modules/commander/Commander.hpp | 2 +- src/modules/commander/commander_params.c | 4 +- .../commander/state_machine_helper.cpp | 3 +- 6 files changed, 15 insertions(+), 38 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 095c369b16..ca47b9939e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 8d74dc0881..851bf96b81 100644 --- a/msg/vehicle_status_flags.msg +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 41243bd30d..e600fda154 100644 --- a/src/modules/commander/Commander.cpp +++ b/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) diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 1fc2fa08e6..3804063281 100644 --- a/src/modules/commander/Commander.hpp +++ b/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 diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 44785b7772..a015e4f4ba 100644 --- a/src/modules/commander/commander_params.c +++ b/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 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 126530d0c0..a552494eb4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/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);