diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b6844a002d..0bb6056169 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1171,7 +1171,8 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); - param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); + param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT"); + param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); @@ -1524,7 +1525,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; - int32_t datalink_loss_enabled = false; + int32_t datalink_loss_enabled = 0; + int32_t rc_loss_enabled = 0; int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; @@ -1608,6 +1610,7 @@ int commander_thread_main(int argc, char *argv[]) /* Safety parameters */ param_get(_param_enable_datalink_loss, &datalink_loss_enabled); + param_get(_param_enable_rc_loss, &rc_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_rc_in_off, &rc_in_off); @@ -2681,11 +2684,12 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, &internal_state, - (bool)datalink_loss_enabled, + (datalink_loss_enabled > 0), mission_result.finished, mission_result.stay_in_failsafe, &status_flags, - land_detector.landed); + land_detector.landed, + (rc_loss_enabled > 0)); if (status.failsafe != failsafe_old) { status_changed = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ed250051a4..ca560a44e9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -92,16 +92,6 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -/** - * Datalink loss mode enabled. - * - * Set to 1 to enable actions triggered when the datalink is lost. - * - * @group Commander - * @boolean - */ -PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); - /** * Datalink loss time threshold * diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a8fb5b3189..8616f0f27f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -585,7 +585,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta */ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe, status_flags_s *status_flags, bool landed) + const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled) { navigation_state_t nav_state_old = status->nav_state; @@ -601,7 +601,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in case commander_state_s::MAIN_STATE_ALTCTL: case commander_state_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { + if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index fd335cc3e7..8a951a0375 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -112,7 +112,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe, status_flags_s *status_flags, bool landed); + const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, + const bool rc_loss_enabled); int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);