From d37bbf1c6e5856f1eaef16f2e1db0a838e99c0f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 May 2021 19:09:41 +0200 Subject: [PATCH] commander: move manual_control and switches out This moves the remaining handling of the manual control stuff out of commander. All communication between manual control now goes through vehicle commands, and the landing gear topic. --- src/modules/commander/CMakeLists.txt | 1 - src/modules/commander/Commander.cpp | 150 +++++----------- src/modules/commander/Commander.hpp | 15 +- src/modules/commander/ManualControl.cpp | 170 ------------------ src/modules/commander/ManualControl.hpp | 101 ----------- src/modules/manual_control/ManualControl.cpp | 1 + .../MulticopterRateControl.cpp | 10 +- .../MulticopterRateControl.hpp | 3 + 8 files changed, 61 insertions(+), 390 deletions(-) delete mode 100644 src/modules/commander/ManualControl.cpp delete mode 100644 src/modules/commander/ManualControl.hpp diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index f9568a0c3ba5..1273dadd8a14 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -50,7 +50,6 @@ 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 92f35a23c9b0..4e9dc78ae085 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -508,14 +508,16 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks) { if (_vehicle_control_mode.flag_control_manual_enabled) { - if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { + if (_vehicle_control_mode.flag_control_climb_rate_enabled && + !_status.rc_signal_lost && _is_throttle_above_center) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center"); tune_negative(true); return TRANSITION_DENIED; } - if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) { + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_status.rc_signal_lost && !_is_throttle_low) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle"); tune_negative(true); return TRANSITION_DENIED; @@ -2188,18 +2190,16 @@ Commander::run() // reset if no longer in LOITER or if manually switched to LOITER const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; - const bool manual_loiter_switch_on = _manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON; - if (!in_loiter_mode || manual_loiter_switch_on) { + if (!in_loiter_mode) { _geofence_loiter_on = false; } // reset if no longer in RTL or if manually switched to RTL const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; - const bool manual_return_switch_on = _manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON; - if (!in_rtl_mode || manual_return_switch_on) { + if (!in_rtl_mode) { _geofence_rtl_on = false; } @@ -2239,37 +2239,54 @@ Commander::run() } } - // Manual control input handling - _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); + manual_control_setpoint_s manual_control_setpoint; - if (_manual_control.update()) { + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + if (manual_control_setpoint.valid) { + if (!_status_flags.rc_signal_found_once) { + _status_flags.rc_signal_found_once = true; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, + _status_flags.rc_calibration_valid, _status); + _status_changed = true; - /* handle the case where RC signal was regained */ - if (!_status_flags.rc_signal_found_once) { - _status_flags.rc_signal_found_once = true; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status); - _status_changed = true; + } else { + if (_status.rc_signal_lost) { + if (_last_valid_manual_control_setpoint > 0) { + mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs", + hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6); + } - } else { - if (_status.rc_signal_lost) { - if (_rc_signal_lost_timestamp > 0) { - mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs", - hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, + _status_flags.rc_calibration_valid, _status); + _status_changed = true; } + } - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status); - _status_changed = true; + _status.rc_signal_lost = false; + _is_throttle_above_center = manual_control_setpoint.z > 0.6f; + _is_throttle_low = manual_control_setpoint.z < 0.1f; + _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; + + } else { + if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) { + if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) { + mavlink_log_critical(&_mavlink_log_pub, "Manual control lost"); + _status.rc_signal_lost = true; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, + false, _status); + _status_changed = true; + } } } - _status.rc_signal_lost = false; // Abort autonomous mode and switch to position mode if sticks are moved significantly // but only if actually in air. if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe && !_geofence_warning_action_on && _armed.armed - && _manual_control.wantsOverride(_vehicle_control_mode)) { + && !_status_flags.rc_input_blocked + && manual_control_setpoint.user_override) { if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state) == TRANSITION_CHANGED) { tune_positive(true); @@ -2277,92 +2294,8 @@ Commander::run() _status_changed = true; } } - - if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) { - // If there's never been a mode change try to use position control as initial state. - _internal_state.desired_main_state = commander_state_s::MAIN_STATE_POSCTL; - } - - if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { - - // handle landing gear switch if configured and in a manual mode - if ((_vehicle_control_mode.flag_control_manual_enabled) && - (_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && - (_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && - (_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) { - // Only switch the landing gear up if the user switched from gear down to gear up. - int8_t gear = landing_gear_s::GEAR_KEEP; - - if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { - gear = landing_gear_s::GEAR_DOWN; - - } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - // gear up ignored unless flying - if (!_land_detector.landed && !_land_detector.maybe_landed) { - gear = landing_gear_s::GEAR_UP; - - } else { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") - } - } - - if (gear != landing_gear_s::GEAR_KEEP) { - landing_gear_s landing_gear{}; - landing_gear.landing_gear = gear; - landing_gear.timestamp = hrt_absolute_time(); - _landing_gear_pub.publish(landing_gear); - } - } - - // evaluate the main state machine according to mode switches - if (set_main_state(_status_changed) == TRANSITION_CHANGED) { - // play tune on mode change only if armed, blink LED always - tune_positive(_armed.armed); - _status_changed = true; - } - } - - /* check throttle kill switch */ - if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { - /* set lockdown flag */ - if (!_armed.manual_lockdown) { - const char kill_switch_string[] = "Kill-switch engaged"; - - if (_land_detector.landed) { - mavlink_log_info(&_mavlink_log_pub, kill_switch_string); - - } else { - mavlink_log_critical(&_mavlink_log_pub, kill_switch_string); - } - - _status_changed = true; - _armed.manual_lockdown = true; - } - - } else if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { - if (_armed.manual_lockdown) { - mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged"); - _status_changed = true; - _armed.manual_lockdown = false; - } - } - - /* no else case: do not change lockdown flag in unconfigured case */ } - if (!_manual_control.isRCAvailable()) { - // set RC lost - if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) { - // ignore RC lost during calibration - if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) { - mavlink_log_critical(&_mavlink_log_pub, "Manual control lost"); - _status.rc_signal_lost = true; - _rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp(); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status); - _status_changed = true; - } - } - } // data link checks which update the status data_link_check(); @@ -2550,6 +2483,9 @@ Commander::run() if (desired_ret == TRANSITION_CHANGED) { // Reset it for next time. _internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX; + + tune_positive(_armed.armed); + _status_changed = true; } } else if (_armed.armed) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6ba97be426ab..5492bac39760 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -37,7 +37,6 @@ /* Helper classes */ #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" -#include "ManualControl.hpp" #include "state_machine_helper.h" #include "worker_thread.hpp" @@ -51,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -71,7 +69,7 @@ #include #include #include -#include +#include #include #include #include @@ -343,10 +341,10 @@ class Commander : public ModuleBase, public ModuleParams unsigned int _leds_counter{0}; - manual_control_switches_s _manual_control_switches{}; - manual_control_switches_s _last_manual_control_switches{}; - ManualControl _manual_control{this}; - hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost + hrt_abstime _last_valid_manual_control_setpoint{0}; + + bool _is_throttle_above_center{false}; + bool _is_throttle_low{false}; hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0}; @@ -390,11 +388,11 @@ class Commander : public ModuleBase, public ModuleParams 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_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)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -422,7 +420,6 @@ class Commander : public ModuleBase, public ModuleParams uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::Publication _mission_pub{ORB_ID(mission)}; - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp deleted file mode 100644 index 1d5df27b89a4..000000000000 --- a/src/modules/commander/ManualControl.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/**************************************************************************** - * - * 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" -#include - -using namespace time_literals; - -enum class OverrideBits : int32_t { - OVERRIDE_AUTO_MODE_BIT = (1 << 0), - OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), -}; - -bool ManualControl::update() -{ - bool updated = false; - - if (_manual_control_setpoint_sub.updated()) { - _last_manual_control_setpoint = _manual_control_setpoint; - _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - - updated = true; - } - - _rc_available = _rc_allowed - && _manual_control_setpoint.valid - && _manual_control_setpoint.timestamp != 0 - && (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s)); - - return updated && _rc_available; -} - -bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode) -{ - const bool override_auto_mode = (_param_rc_override.get() & static_cast(OverrideBits::OVERRIDE_AUTO_MODE_BIT)) - && vehicle_control_mode.flag_control_auto_enabled; - - const bool override_offboard_mode = (_param_rc_override.get() & static_cast - (OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)) - && vehicle_control_mode.flag_control_offboard_enabled; - - if (_rc_available && (override_auto_mode || override_offboard_mode)) { - - return _manual_control_setpoint.user_override; - } - - return false; -} - -bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, - const vehicle_status_s &vehicle_status, - manual_control_switches_s &manual_control_switches, const bool landed) -{ - bool ret = false; - - const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE; - const bool use_button = !use_stick && _param_com_arm_swisbtn.get(); - const bool use_switch = !use_stick && !use_button; - - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool stick_in_lower_left = use_stick - && isThrottleLow() - && _manual_control_setpoint.r < -.9f; - const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) - && use_button; - const bool arm_switch_to_disarm_transition = use_switch - && (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON) - && (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF); - const bool mc_manual_thrust_mode = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && vehicle_control_mode.flag_control_manual_enabled - && !vehicle_control_mode.flag_control_climb_rate_enabled; - - if (armed - && (landed || mc_manual_thrust_mode) - && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { - - const bool last_disarm_hysteresis = _stick_disarm_hysteresis.get_state(); - _stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time()); - const bool disarm_trigger = !last_disarm_hysteresis && _stick_disarm_hysteresis.get_state() - && !_stick_arm_hysteresis.get_state(); - - if (disarm_trigger || arm_switch_to_disarm_transition) { - ret = true; - } - - } else if (!arm_button_pressed) { - - _stick_disarm_hysteresis.set_state_and_update(false, hrt_absolute_time()); - } - - return ret; -} - -bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, - const manual_control_switches_s &manual_control_switches, const bool landed) -{ - bool ret = false; - - const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE; - const bool use_button = !use_stick && _param_com_arm_swisbtn.get(); - const bool use_switch = !use_stick && !use_button; - - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool stick_in_lower_right = use_stick - && isThrottleLow() - && _manual_control_setpoint.r > .9f; - const bool arm_button_pressed = use_button - && (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON); - const bool arm_switch_to_arm_transition = use_switch - && (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF) - && (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON); - - if (!armed - && (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) { - - const bool last_arm_hysteresis = _stick_arm_hysteresis.get_state(); - _stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time()); - const bool arm_trigger = !last_arm_hysteresis && _stick_arm_hysteresis.get_state() - && !_stick_disarm_hysteresis.get_state(); - - if (arm_trigger || arm_switch_to_arm_transition) { - ret = true; - } - - } else if (!arm_button_pressed) { - - _stick_arm_hysteresis.set_state_and_update(false, hrt_absolute_time()); - } - - _last_manual_control_switches_arm_switch = manual_control_switches.arm_switch; // After disarm and arm check - - return ret; -} - -void ManualControl::updateParams() -{ - ModuleParams::updateParams(); - _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); - _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); -} diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp deleted file mode 100644 index 38a616f5ffc4..000000000000 --- a/src/modules/commander/ManualControl.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include -#include -#include - -class ManualControl : ModuleParams -{ -public: - ManualControl(ModuleParams *parent) : ModuleParams(parent) {}; - ~ManualControl() override = default; - - void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; } - - /** - * Check for manual control input changes and process them - * @return true if there was new data - */ - bool update(); - bool isRCAvailable() const { return _rc_available; } - bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; } - bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); - bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, - manual_control_switches_s &manual_control_switches, const bool landed); - bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, - const manual_control_switches_s &manual_control_switches, const bool landed); - bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; } - bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; } - hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; } - -private: - void updateParams() override; - - 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{}; - - // Availability - bool _rc_allowed{false}; - bool _rc_available{false}; - - // Arming/disarming - systemlib::Hysteresis _stick_disarm_hysteresis{false}; - systemlib::Hysteresis _stick_arm_hysteresis{false}; - uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_com_rc_loss_t, - (ParamInt) _param_rc_arm_hyst, - (ParamBool) _param_com_arm_swisbtn, - (ParamBool) _param_com_rearm_grace, - (ParamInt) _param_rc_override, - (ParamFloat) _param_com_rc_stick_ov - ) -}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index deb3ac7b86ee..ccad4420e28d 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -215,6 +215,7 @@ void ManualControl::Run() } if (switches.gear_switch != _previous_switches.gear_switch) { + if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { publish_landing_gear(landing_gear_s::GEAR_UP); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index aa3c46b55139..34cbcb30a7db 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -153,8 +153,14 @@ MulticopterRateControl::Run() landing_gear_s landing_gear; if (_landing_gear_sub.copy(&landing_gear)) { - if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - _landing_gear = landing_gear.landing_gear; + if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP && + _v_control_mode.flag_control_manual_enabled) { + if (landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) { + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") + + } else { + _landing_gear = landing_gear.landing_gear; + } } } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 531b1e435bd5..13dde483bdac 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,8 @@ class MulticopterRateControl : public ModuleBase, public uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + orb_advert_t _mavlink_log_pub{nullptr}; + vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{};