From ced83762689d900a448074b65b25e2344a17f1e9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 26 Nov 2015 17:45:20 +0100 Subject: [PATCH] added offboard lost actions with additional timeout --- posix-configs/SITL/init/rcS_gazebo_iris | 6 +- src/modules/commander/commander.cpp | 25 ++++++- src/modules/commander/commander_params.c | 12 +++ .../commander/state_machine_helper.cpp | 73 +++++++++++++++++-- src/modules/commander/state_machine_helper.h | 3 +- src/modules/navigator/navigator_params.c | 30 ++++++++ 6 files changed, 138 insertions(+), 11 deletions(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 5978c609de..46ab10febb 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -23,9 +23,11 @@ param set COM_RC_IN_MODE 1 param set NAV_DLL_ACT 2 param set COM_DISARM_LAND 3 param set NAV_ACC_RAD 2.0 +param set COM_OF_LOSS_T 5 param set RTL_RETURN_ALT 30.0 -param set RTL_DESCEND_ALT 10.0 -param set RTL_LAND_DELAY 0 +param set RTL_DESCEND_ALT 5.0 +param set RTL_LAND_DELAY 5 +param set COM_DISARM_LAND 5 param set MIS_TAKEOFF_ALT 2.5 param set MC_ROLLRATE_P 0.3 param set MC_PITCHRATE_P 0.3 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index afb8a11f7b..c010b7c6b2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1161,6 +1161,8 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); 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_offboard_loss_act = param_find("NAV_OBL_ACT"); + param_t _param_offboard_loss_rc_act = param_find("NAV_OBL_RC_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"); @@ -1176,6 +1178,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_geofence_action = param_find("GF_ACTION"); param_t _param_disarm_land = param_find("COM_DISARM_LAND"); param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT"); + param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T"); param_t _param_fmode_1 = param_find("COM_FLTMODE1"); param_t _param_fmode_2 = param_find("COM_FLTMODE2"); @@ -1256,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_flags.offboard_control_signal_lost = true; status.data_link_lost = true; + status_flags.offboard_control_loss_timeout = false; status_flags.condition_system_prearm_error_reported = false; status_flags.condition_system_hotplug_timeout = false; @@ -1515,6 +1519,9 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; + float offboard_loss_timeout = 0.0f; + int32_t offboard_loss_act = 0; + int32_t offboard_loss_rc_act = 0; int32_t geofence_action = 0; @@ -1609,6 +1616,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_geofence_action, &geofence_action); param_get(_param_disarm_land, &disarm_when_landed); param_get(_param_low_bat_act, &low_bat_action); + param_get(_param_offboard_loss_timeout, &offboard_loss_timeout); + param_set(_param_offboard_loss_act, &offboard_loss_act); + param_set(_param_offboard_loss_rc_act, &offboard_loss_rc_act); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1651,12 +1661,23 @@ int commander_thread_main(int argc, char *argv[]) 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; } } else { if (!status_flags.offboard_control_signal_lost) { status_flags.offboard_control_signal_lost = true; + + if (offboard_loss_timeout < 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 + offboard_loss_timeout * 1e6f > hrt_absolute_time(); + } status_changed = true; } } @@ -2669,7 +2690,9 @@ int commander_thread_main(int argc, char *argv[]) mission_result.stay_in_failsafe, &status_flags, land_detector.landed, - (rc_loss_enabled > 0)); + (rc_loss_enabled > 0), + offboard_loss_act, + offboard_loss_rc_act); 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 0402554493..3ca993071b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -413,3 +413,15 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1); * @value 12 Follow Me */ PARAM_DEFINE_INT32(COM_FLTMODE6, -1); + +/** + * Time-out to wait when offboard connection is lost before triggering offboard lost action. + * See NAV_OBL_ACT and NAV_OBL_RC_ACT to configure action. + * + * @group Commander + * @unit second + * @min 0 + * @max 60 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 88f3e5d6ec..8eb5d4fc3e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -588,7 +588,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 rc_loss_enabled) + const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled, + const int offb_loss_act, const int offb_loss_rc_act) { navigation_state_t nav_state_old = status->nav_state; @@ -847,17 +848,75 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) { + if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid + && offb_loss_rc_act == 3) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + + } else if (status_flags->condition_local_altitude_valid && offb_loss_rc_act == 1) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else if (offb_loss_rc_act == 2) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + + } else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 4) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + } else { + if (status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } + } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; - if (status_flags->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } else if (status_flags->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) { + if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid + && offb_loss_rc_act == 2) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 1) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + } else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + if (status_flags->condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + } else if (status_flags->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } } + } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f3302eba08..4012c0e89f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -81,6 +81,7 @@ struct status_flags_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; 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_signal_lost_cmd; // true if RC lost mode is commanded bool rc_input_blocked; // set if RC input should be ignored temporarily @@ -113,7 +114,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 rc_loss_enabled); + const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act); 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); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 523f03b42e..ab923c4850 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -104,6 +104,36 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0); */ PARAM_DEFINE_INT32(NAV_RCL_ACT, 0); +/** + * Set offboard loss failsafe mode + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value 0 Land at current position + * @value 1 Loiter + * @value 2 Return to Land + * + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_OBL_ACT, 0); + +/** + * Set offboard loss failsafe mode when RC is available + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value 0 Position control + * @value 1 Altitude control + * @value 2 Manual + * @value 3 Return to Land + * @value 4 Land at current position + * + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_OBL_RC_ACT, 0); + /** * Airfield home Lat *