Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

commander: move manual_control and switches out

Browse Source 
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.
This commit is contained in:
Julian Oes
2021-05-11 19:09:41 +02:00
committed by Matthias Grob
parent e49b596edc
commit 97d01f200e
8 changed files with 64 additions and 410 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
-1
View File
@@ -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
src/modules/commander/Commander.cpp
+45 -112
View File
@@ -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<float>(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<float>(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) {
src/modules/commander/Commander.hpp
+7 -10
View File
@@ -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 <uORB/topics/actuator_armed.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -71,7 +69,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
@@ -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_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
src/modules/commander/ManualControl.cpp
-183
View File
@@ -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 <drivers/drv_hrt.h>
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<int32_t>(OverrideBits::OVERRIDE_AUTO_MODE_BIT))
&& vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (_param_rc_override.get() & static_cast<int32_t>
(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);
}
src/modules/commander/ManualControl.hpp
-102
View File
@@ -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 <maetugr@gmail.com>
*/
#pragma once
#include <lib/hysteresis/hysteresis.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
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<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons
)
};
src/modules/manual_control/ManualControl.cpp
+1
View File
@@ -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);
src/modules/mc_rate_control/MulticopterRateControl.cpp
+8 -2
View File
@@ -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;
}
}
}
}
src/modules/mc_rate_control/MulticopterRateControl.hpp
+3
View File
@@ -42,6 +42,7 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
@@ -118,6 +119,8 @@ private:
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _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{};