From cdadfabccc5f67981eec9656da04d0d9557e4c43 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 19 Nov 2020 11:10:10 +0100 Subject: [PATCH] Bring back RC throttle override with a parameter to disable it --- src/modules/commander/Commander.cpp | 10 +++++++--- src/modules/commander/Commander.hpp | 4 ++-- src/modules/commander/commander_params.c | 7 ++++--- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5b04e6bcc8..8d94b489e2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2107,10 +2107,14 @@ Commander::run() if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) { const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get(); - if (!_status.rc_signal_lost && - ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || - (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { + const bool rpy_deflected = (fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || + (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection) + || (fabsf(_manual_control_setpoint.r) > minimum_stick_deflection); + const bool throttle_deflected = fabsf(_manual_control_setpoint.z - 0.5f) * 2.f > minimum_stick_deflection; + const bool use_throttle = !(_param_rc_override.get() & OVERRIDE_IGNORE_THROTTLE_BIT); + if (!_status.rc_signal_lost && + (rpy_deflected || (use_throttle && throttle_deflected))) { if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state) == TRANSITION_CHANGED) { tune_positive(true); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4574f1c741..2fb24a8331 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -287,9 +287,9 @@ private: }; enum OverrideMode { - OVERRIDE_DISABLED = 0, OVERRIDE_AUTO_MODE_BIT = (1 << 0), - OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1) + OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), + OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) }; /* Decouple update interval and hysteresis counters, all depends on intervals */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f0b4eb1605..98aa3ae8c4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -638,9 +638,10 @@ PARAM_DEFINE_INT32(COM_REARM_GRACE, 1); * Note: Only has an effect on multicopters, and VTOLs in multicopter mode. * * @min 0 - * @max 3 + * @max 7 * @bit 0 Enable override during auto modes (except for in critical battery reaction) * @bit 1 Enable override during offboard mode + * @bit 2 Ignore throttle stick * @group Commander */ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1); @@ -648,8 +649,8 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1); /** * RC stick override threshold * - * If COM_RC_OVERRIDE is enabled and the joystick input controlling the horizontally axis (right stick for RC in mode 2) - * is moved more than this threshold from the center the autopilot switches to position mode and the pilot takes over control. + * If COM_RC_OVERRIDE is enabled and the joystick input is moved more than this threshold from the center + * the autopilot switches to position mode and the pilot takes over control. * * @group Commander * @unit %