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{};