From dda895c94b4ed7f4c1649c5cca443a02f29860c8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 16 Feb 2021 08:18:30 +0100 Subject: [PATCH] Commander: split out rc override logic into ManualControl --- src/modules/commander/CMakeLists.txt | 1 + src/modules/commander/Commander.cpp | 41 ++++-------- src/modules/commander/Commander.hpp | 10 +-- src/modules/commander/ManualControl.cpp | 84 +++++++++++++++++++++++++ src/modules/commander/ManualControl.hpp | 64 +++++++++++++++++++ 5 files changed, 163 insertions(+), 37 deletions(-) create mode 100644 src/modules/commander/ManualControl.cpp create mode 100644 src/modules/commander/ManualControl.hpp diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 1273dadd8a..f9568a0c3b 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -50,6 +50,7 @@ px4_add_module( level_calibration.cpp lm_fit.cpp mag_calibration.cpp + ManualControl.cpp rc_calibration.cpp state_machine_helper.cpp worker_thread.cpp diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f69329319b..972078432d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1991,8 +1991,8 @@ Commander::run() } } - // update manual_control_setpoint before geofence (which might check sticks or switches) - _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _manual_control.update(); + _manual_control_setpoint = _manual_control._manual_control_setpoint; /* start geofence result check */ _geofence_result_sub.update(&_geofence_result); @@ -2096,33 +2096,16 @@ Commander::run() } // abort autonomous mode and switch to position mode if sticks are moved significantly - if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { - - const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) - && _vehicle_control_mode.flag_control_auto_enabled; - - const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) - && _vehicle_control_mode.flag_control_offboard_enabled; - - if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) { - const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get(); - - const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change) - || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change) - || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change); - const bool throttle_moved = - (fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change); - const bool use_throttle = !(_param_rc_override.get() & OVERRIDE_IGNORE_THROTTLE_BIT); - - if (!_status.rc_signal_lost && - (rpy_moved || (use_throttle && throttle_moved))) { - if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, - &_internal_state) == TRANSITION_CHANGED) { - tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); - _status_changed = true; - } - } + if ((_param_rc_override.get() != 0) + && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && !in_low_battery_failsafe && !_geofence_warning_action_on + && _manual_control.wantsOverride(_param_rc_override.get(), _param_com_rc_stick_ov.get(), _vehicle_control_mode, + !_status.rc_signal_lost)) { + if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, + &_internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + _status_changed = true; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f6831ee22a..df3a43178c 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -37,6 +37,7 @@ /* Helper classes */ #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" +#include "ManualControl.hpp" #include "state_machine_helper.h" #include "worker_thread.hpp" @@ -70,7 +71,6 @@ #include #include #include -#include #include #include #include @@ -286,12 +286,6 @@ private: ALWAYS = 2 }; - enum OverrideMode { - OVERRIDE_AUTO_MODE_BIT = (1 << 0), - OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), - OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) - }; - /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)}; @@ -367,6 +361,7 @@ private: manual_control_setpoint_s _last_manual_control_setpoint{}; manual_control_switches_s _manual_control_switches{}; manual_control_switches_s _last_manual_control_switches{}; + ManualControl _manual_control; hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {}; uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE}; @@ -417,7 +412,6 @@ private: uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp new file mode 100644 index 0000000000..8539c2f495 --- /dev/null +++ b/src/modules/commander/ManualControl.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ManualControl.hpp" + +enum OverrideBits { + OVERRIDE_AUTO_MODE_BIT = (1 << 0), + OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), + OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) +}; + +void ManualControl::update() +{ + if (_manual_control_setpoint_sub.updated()) { + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + process(manual_control_setpoint); + } + } +} + +void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint) +{ + _last_manual_control_setpoint = _manual_control_setpoint; + _manual_control_setpoint = manual_control_setpoint; +} + +bool ManualControl::wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov, + const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available) +{ + const bool override_auto_mode = (param_rc_override & OverrideBits::OVERRIDE_AUTO_MODE_BIT) + && vehicle_control_mode.flag_control_auto_enabled; + + const bool override_offboard_mode = (param_rc_override & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) + && vehicle_control_mode.flag_control_offboard_enabled; + + if (rc_available && (override_auto_mode || override_offboard_mode)) { + const float minimum_stick_change = .01f * param_com_rc_stick_ov; + + const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change) + || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change) + || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change); + const bool throttle_moved = + (fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change); + const bool use_throttle = !(param_rc_override & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + + if (rpy_moved || (use_throttle && throttle_moved)) { + return true; + } + } + + return false; +} diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp new file mode 100644 index 0000000000..c71540e633 --- /dev/null +++ b/src/modules/commander/ManualControl.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ManualControl.hpp + * + * @brief Logic for handling RC or Joystick input + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include + +class ManualControl +{ +public: + ManualControl() = default; + ~ManualControl() = default; + + void update(); + bool wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov, + const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available); + +//private: + void process(manual_control_setpoint_s &manual_control_setpoint); + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + manual_control_setpoint_s _manual_control_setpoint{}; + manual_control_setpoint_s _last_manual_control_setpoint{}; +};