From 18d823f9e39e1722e9215effa68bccc30f9aa6d6 Mon Sep 17 00:00:00 2001 From: Tomas Twardzik Date: Tue, 14 Jan 2025 11:58:19 +0100 Subject: [PATCH] [add] throttle high fix for boats and rovers --- src/modules/commander/Commander.cpp | 10 +++++++--- src/modules/commander/Commander.hpp | 1 + 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e4fc8ca921..f9285c3011 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -572,9 +572,12 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ return TRANSITION_DENIED; } - if (!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low - && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { + if ((!_failsafe_flags.manual_control_signal_lost) && + ((!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_is_throttle_low) || + ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER || + _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) && + !_is_throttle_near_center))) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, @@ -2932,6 +2935,7 @@ void Commander::manualControlCheck() _is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f); _is_throttle_low = (manual_control_setpoint.throttle < -0.8f); + _is_throttle_near_center = (fabsf(manual_control_setpoint.throttle) < 0.05f); if (isArmed()) { // Abort autonomous mode and switch to position mode if sticks are moved significantly diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 04c1a0d05a..f4d1b5eaae 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -277,6 +277,7 @@ private: bool _is_throttle_above_center{false}; bool _is_throttle_low{false}; + bool _is_throttle_near_center{false}; bool _arm_tune_played{false}; bool _have_taken_off_since_arming{false};