From e31304d7d59c3967c132cd94fbb4adc440958ee9 Mon Sep 17 00:00:00 2001 From: Jonas Perolini <74473718+JonasPerolini@users.noreply.github.com> Date: Thu, 24 Mar 2022 10:27:32 +0100 Subject: [PATCH] Quadchute param update (#19351) New parameter for actions after a quadchute (COM_QC_ACT) Co-authored-by: Jonas Co-authored-by: Matthias Grob --- src/modules/commander/Commander.cpp | 1 + src/modules/commander/Commander.hpp | 3 ++ src/modules/commander/commander_params.c | 11 +++++ .../commander/state_machine_helper.cpp | 49 +++++++++++++++++-- src/modules/commander/state_machine_helper.h | 8 +++ 5 files changed, 69 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3a2761792f..0f98af9dfe 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2881,6 +2881,7 @@ Commander::run() _vehicle_land_detected.landed, static_cast(_param_nav_rcl_act.get()), static_cast(_param_com_obl_act.get()), + static_cast(_param_com_qc_act.get()), static_cast(_param_com_obl_rc_act.get()), static_cast(_param_com_posctl_navl.get()), _param_com_rcl_act_t.get(), diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f9d4f11ba2..62d1e05260 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -223,6 +223,9 @@ private: (ParamInt) _param_rc_map_fltmode, (ParamInt) _param_rc_map_mode_sw, + // Quadchute + (ParamInt) _param_com_qc_act, + // Offboard (ParamFloat) _param_com_of_loss_t, (ParamInt) _param_com_obl_act, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f36ad0c096..d2e672ea25 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -416,6 +416,17 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f); */ PARAM_DEFINE_INT32(COM_OBL_ACT, 0); +/** + * Set command after a quadchute + * + * @value -1 No action: stay in current flight mode + * @value 0 Return mode + * @value 1 Land mode + * @value 2 Hold mode + * @group Commander + */ +PARAM_DEFINE_INT32(COM_QC_ACT, 0); + /** * Set offboard loss failsafe mode when RC is available * diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0e1f550e8e..109f8d24a1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -147,6 +147,10 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm const vehicle_status_flags_s &status_flags, const offboard_loss_actions_t offboard_loss_act); +void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const quadchute_actions_t quadchute_act); + void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed, const vehicle_status_flags_s &status_flags, const offboard_loss_rc_actions_t offboard_loss_rc_act); @@ -497,6 +501,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, + const quadchute_actions_t quadchute_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_rcl_act_t, const int param_com_rcl_except) @@ -588,7 +593,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status_flags.vtol_transition_failure) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + set_quadchute_nav_state(status, armed, status_flags, quadchute_act); } else if (status.mission_failure) { status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; @@ -641,7 +647,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // nothing to do - everything done in check_invalid_pos_nav_state } else if (status_flags.vtol_transition_failure) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + set_quadchute_nav_state(status, armed, status_flags, quadchute_act); } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { // Data link lost, data link loss reaction configured -> do configured reaction @@ -756,7 +763,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // nothing to do - everything done in check_invalid_pos_nav_state } else if (status_flags.vtol_transition_failure) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + set_quadchute_nav_state(status, armed, status_flags, quadchute_act); } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { // Data link lost, data link loss reaction configured -> do configured reaction @@ -985,6 +993,41 @@ void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, c } } +void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const quadchute_actions_t quadchute_act) +{ + switch (quadchute_act) { + case quadchute_actions_t::NO_ACTION: + // If quadchute action is disabled then no action must be taken. + break; + + default: + case quadchute_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } + + break; + + // FALLTHROUGH + case quadchute_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } + + break; + + // FALLTHROUGH + case quadchute_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + + break; + } +} + void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, const vehicle_status_flags_s &status_flags, const offboard_loss_actions_t offboard_loss_act) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 995c0d4fb3..45806db1d6 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -70,6 +70,13 @@ enum class link_loss_actions_t { LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values) }; +enum class quadchute_actions_t { + NO_ACTION = -1, + AUTO_RTL = 0, // Return mode + AUTO_LAND = 1, // Land mode + AUTO_LOITER = 2, // Hold mode +}; + enum class offboard_loss_actions_t { DISABLED = -1, AUTO_LAND = 0, // Land mode @@ -122,6 +129,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, + const quadchute_actions_t quadchute_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_rcl_act_t, const int param_com_rcl_except);