diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 255bc0b1d6..bda82b74f4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2546,7 +2546,7 @@ Commander::run() (offboard_loss_actions_t)_param_com_obl_act.get(), (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(), (position_nav_loss_actions_t)_param_com_posctl_navl.get(), - _param_com_ll_delay.get()); + _param_com_rcl_act_t.get()); if (nav_state_changed) { _status.nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 576ac7655d..e171fc0199 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -189,13 +189,13 @@ private: (ParamInt) _param_nav_dll_act, (ParamInt) _param_com_dl_loss_t, - (ParamFloat) _param_com_ll_delay, (ParamInt) _param_com_hldl_loss_t, (ParamInt) _param_com_hldl_reg_t, (ParamInt) _param_nav_rcl_act, (ParamFloat) _param_com_rc_loss_t, + (ParamFloat) _param_com_rcl_act_t, (ParamFloat) _param_com_home_h_t, (ParamFloat) _param_com_home_v_t, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 9795f1f5bc..ca8f0de3f8 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -174,7 +174,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); /** * RC loss time threshold * - * After this amount of seconds without RC connection the rc lost flag is set to true + * After this amount of seconds without RC connection it's considered lost and not used anymore * * @group Commander * @unit s @@ -185,6 +185,23 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); +/** + * Delay between RC loss and configured reaction + * + * RC signal not updated -> still use data for COM_RC_LOSS_T seconds + * Consider RC signal lost -> wait COM_RCL_ACT_T seconds on the spot waiting to regain signal + * React with failsafe action NAV_RCL_ACT + * + * A zero value disables the delay. + * + * @group Commander + * @unit s + * @min 0.0 + * @max 25.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f); + /** * Home set horizontal threshold * @@ -968,18 +985,3 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); * @value 1 Enabled */ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); - -/** - * Delay between RC loss and configured reaction - * - * A non-zero, positive value makes the failsafe reaction first stop the vehicle and wait - * before proceeding with the configured failsafe reaction NAV_RCL_ACT. - * A zero or negative value disables the delay. - * - * @group Commander - * @unit s - * @min -1.0 - * @max 25.0 - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(COM_LL_DELAY, 15.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e37c4aa788..b0a30b6ae7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -409,7 +409,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_ll_delay) + const float param_com_rcl_act_t) { const navigation_state_t nav_state_old = status->nav_state; @@ -438,7 +438,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ if (rc_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_ll_delay); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else { switch (internal_state->main_state) { @@ -474,7 +474,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ if (rc_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_ll_delay); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ /* A local position estimate is enough for POSCTL for multirotors, diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0adb54602d..fc28a54de1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -132,7 +132,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_ll_delay); + const float param_com_rcl_act_t); /* * Checks the validty of position data against the requirements of the current navigation