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