From 4c9288d993982218b361fad4c2b5c30dee655a2c Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 15 Aug 2019 12:20:18 +0200 Subject: [PATCH] Commander: cleanup COM_POSCTL_NAVL parameter - move to px4::params - use enum --- src/modules/commander/Commander.cpp | 9 +-------- src/modules/commander/Commander.hpp | 1 + src/modules/commander/commander_params.c | 4 ++-- src/modules/commander/state_machine_helper.cpp | 5 +++-- src/modules/commander/state_machine_helper.h | 7 ++++++- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a6ba45a7e5..17dc911d51 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1243,9 +1243,6 @@ Commander::run() param_t _param_airmode = param_find("MC_AIRMODE"); param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW"); - /* failsafe response to loss of navigation accuracy */ - param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL"); - status_flags.avoidance_system_required = _param_com_obs_avoid.get(); /* pthread for slow low prio thread */ @@ -1374,7 +1371,6 @@ Commander::run() param_get(_param_rc_arm_hyst, &rc_arm_hyst); rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; - int32_t posctl_nav_loss_act = 0; int32_t geofence_action = 0; int32_t flight_uuid = 0; int32_t airmode = 0; @@ -1508,9 +1504,6 @@ Commander::run() param_get(_param_fmode_5, &_flight_mode_slots[4]); param_get(_param_fmode_6, &_flight_mode_slots[5]); - /* failsafe response to loss of navigation accuracy */ - param_get(_param_posctl_nav_loss_act, &posctl_nav_loss_act); - param_get(_param_takeoff_finished_action, &takeoff_complete_act); /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */ @@ -2317,7 +2310,7 @@ Commander::run() (link_loss_actions_t)_param_nav_rcl_act.get(), _param_com_obl_act.get(), _param_com_obl_rc_act.get(), - posctl_nav_loss_act); + (position_nav_loss_actions_t)_param_com_posctl_navl.get()); if (status.failsafe != failsafe_old) { status_changed = true; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f3da471d4c..b4f8901dba 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -116,6 +116,7 @@ private: (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_pos_fs_epv, (ParamFloat) _param_com_vel_fs_evh, + (ParamInt) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ (ParamInt) _param_com_pos_fs_delay, (ParamInt) _param_com_pos_fs_prob, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 372e1d14e5..59b052397d 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -649,8 +649,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control. * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. * - * @value 0 Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. - * @value 1 Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. + * @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + * @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. * * @group Commander */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9c6e9cbfff..0124deea7e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -393,7 +393,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 int offb_loss_act, const int offb_loss_rc_act, - const int posctl_nav_loss_act) + const position_nav_loss_actions_t posctl_nav_loss_act) { navigation_state_t nav_state_old = status->nav_state; @@ -468,7 +468,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ * For fixedwing, a global position is needed. */ } else if (is_armed - && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), + && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, + !(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE), status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { // nothing to do - everything done in check_invalid_pos_nav_state diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a07e8c16b6..879f3a0eb4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -68,6 +68,11 @@ enum class link_loss_actions_t { LOCKDOWN = 6, // Kill the motors, same result as kill switch }; +enum class position_nav_loss_actions_t { + ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. +}; + typedef enum { ARM_REQ_NONE = 0, ARM_REQ_MISSION_BIT = (1 << 0), @@ -95,7 +100,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 int offb_loss_act, const int offb_loss_rc_act, - const int posctl_nav_loss_act); + const position_nav_loss_actions_t posctl_nav_loss_act); /* * Checks the validty of position data aaainst the requirements of the current navigation