diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 7b70b6e2fec..9a68a8f1841 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 60110848c76..1bd9ff26807 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -556,7 +556,8 @@ 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\t"); events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info}, @@ -566,7 +567,8 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } - 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\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, @@ -2307,18 +2309,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; } @@ -2364,39 +2364,56 @@ 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) { + float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f; + mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed); + events::send(events::ID("commander_rc_regained"), events::Log::Info, + "Manual control regained after {1:.1} s", elapsed); + } + + 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.rc_signal_lost) { - if (_rc_signal_lost_timestamp > 0) { - float elapsed = hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6f; - mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed); - events::send(events::ID("commander_rc_regained"), events::Log::Info, - "Manual control regained after {1:.1} s", elapsed); + 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; } - - 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; // 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)) { + && !_status_flags.rc_input_blocked + && manual_control_setpoint.user_override) { const transition_result_t posctl_result = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); @@ -2421,93 +2438,6 @@ Commander::run() } } } - - if (!_armed.armed && _manual_control.isModeInitializationRequired() && (_internal_state.main_state_changes == 0)) { - // if there's never been a mode change force position control as initial state - _internal_state.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\t") - events::send(events::ID("commander_landed_landing_gear_error"), events::Log::Critical, - "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); - } - } - } - - /* 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\t"; - events::LogLevels log_levels{events::Log::Info}; - - if (_land_detector.landed) { - mavlink_log_info(&_mavlink_log_pub, kill_switch_string); - - } else { - mavlink_log_critical(&_mavlink_log_pub, kill_switch_string); - log_levels.external = events::Log::Critical; - } - - events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill-switch engaged"); - - _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\t"); - events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "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\t"); - events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, - "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 @@ -2732,6 +2662,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 03fd46627b0..09b3fd71066 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -36,7 +36,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 @@ -347,10 +345,10 @@ private: 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,12 +388,12 @@ private: uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; 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)}; @@ -429,7 +427,6 @@ private: 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 e4089c2290b..00000000000 --- a/src/modules/commander/ManualControl.cpp +++ /dev/null @@ -1,183 +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 vehicle_status_s &vehicle_status) -{ - 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; - - // in Descend manual override is enbaled independently of COM_RC_OVERRIDE - const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND); - - - if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) { - - 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; -} - -bool ManualControl::isModeInitializationRequired() -{ - const bool is_mavlink = _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; - const bool rc_uses_toggle_buttons = _param_rc_map_flightmode_buttons.get() > 0; - - return (is_mavlink || rc_uses_toggle_buttons); -} - -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 cce7c118d64..00000000000 --- a/src/modules/commander/ManualControl.hpp +++ /dev/null @@ -1,102 +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 wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status); - 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 isModeInitializationRequired(); - 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, - (ParamInt) _param_rc_map_flightmode_buttons - ) -}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index deb3ac7b86e..ccad4420e28 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 f79b64e26b1..0a7dd7bdfff 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 6f8ef98288d..77a72e4fc39 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 @@ -118,6 +119,8 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + orb_advert_t _mavlink_log_pub{nullptr}; + vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{};