From 87415d36a21cf9ed3a7cc1de332aa9eb7db0df90 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 30 Oct 2019 12:02:31 +0100 Subject: [PATCH] commander: check center throttle for POSCTL/ALTCTL When flying POSCTL and ALTCTL the throttle stick is usually spring loaded and therefore centered. Therefore, it makes more sense to check for above center instead of above low. --- src/modules/commander/Commander.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0fd5e12655..00f5accdf5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -837,17 +837,23 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; } - // Refuse to arm if in manual with non-zero throttle - if (cmd_arms - && (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL - || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL - || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL - || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO - || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB - || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE) - && (sp_man.z > 0.1f)) { + const bool throttle_above_low = (sp_man.z > 0.1f); + const bool throttle_above_center = (sp_man.z > 0.6f); - mavlink_log_critical(&mavlink_log_pub, "Arming denied! Manual throttle not zero"); + if (cmd_arms && throttle_above_center && + (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || + status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { + mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not centered"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + break; + } + + if (cmd_arms && throttle_above_low && + (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || + status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || + status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || + status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) { + mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not zero"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; break; }