fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: split out rc override logic into ManualControl

Browse Source
This commit is contained in:
Matthias Grob 2021-02-16 08:18:30 +01:00
parent a457710d4c
commit bfd830e580
5 changed files with 163 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/commander/CMakeLists.txt
View File

@ -50,6 +50,7 @@ px4_add_module(
level_calibration.cpp
lm_fit.cpp
mag_calibration.cpp
ManualControl.cpp
rc_calibration.cpp
state_machine_helper.cpp
worker_thread.cpp

41
src/modules/commander/Commander.cpp
View File

@ -2001,8 +2001,8 @@ Commander::run()
}
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control.update();
_manual_control_setpoint = _manual_control._manual_control_setpoint;
/* start geofence result check */
_geofence_result_sub.update(&_geofence_result);
@ -2106,33 +2106,16 @@ Commander::run()
}
// abort autonomous mode and switch to position mode if sticks are moved significantly
if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT)
&& _vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT)
&& _vehicle_control_mode.flag_control_offboard_enabled;
if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) {
const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
const bool throttle_moved =
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
const bool use_throttle = !(_param_rc_override.get() & OVERRIDE_IGNORE_THROTTLE_BIT);
if (!_status.rc_signal_lost &&
(rpy_moved || (use_throttle && throttle_moved))) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
&_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
_status_changed = true;
}
}
if ((_param_rc_override.get() != 0)
&& (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on
&& _manual_control.wantsOverride(_param_rc_override.get(), _param_com_rc_stick_ov.get(), _vehicle_control_mode,
!_status.rc_signal_lost)) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
&_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
_status_changed = true;
}
}

10
src/modules/commander/Commander.hpp
View File

@ -37,6 +37,7 @@
/* Helper classes */
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "ManualControl.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"
@ -70,7 +71,6 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
@ -286,12 +286,6 @@ private:
ALWAYS = 2
};
enum OverrideMode {
OVERRIDE_AUTO_MODE_BIT = (1 << 0),
OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
};
/* Decouple update interval and hysteresis counters, all depends on intervals */
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)};
@ -367,6 +361,7 @@ private:
manual_control_setpoint_s _last_manual_control_setpoint{};
manual_control_switches_s _manual_control_switches{};
manual_control_switches_s _last_manual_control_switches{};
ManualControl _manual_control;
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
@ -417,7 +412,6 @@ private:
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};

84
src/modules/commander/ManualControl.cpp Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ManualControl.hpp"
enum OverrideBits {
OVERRIDE_AUTO_MODE_BIT = (1 << 0),
OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
};
void ManualControl::update()
{
if (_manual_control_setpoint_sub.updated()) {
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
process(manual_control_setpoint);
}
}
}
void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint)
{
_last_manual_control_setpoint = _manual_control_setpoint;
_manual_control_setpoint = manual_control_setpoint;
}
bool ManualControl::wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov,
const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available)
{
const bool override_auto_mode = (param_rc_override & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
&& vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (param_rc_override & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
&& vehicle_control_mode.flag_control_offboard_enabled;
if (rc_available && (override_auto_mode || override_offboard_mode)) {
const float minimum_stick_change = .01f * param_com_rc_stick_ov;
const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
const bool throttle_moved =
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
const bool use_throttle = !(param_rc_override & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
if (rpy_moved || (use_throttle && throttle_moved)) {
return true;
}
}
return false;
}

64
src/modules/commander/ManualControl.hpp Normal file
View File

@ -0,0 +1,64 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ManualControl.hpp
*
* @brief Logic for handling RC or Joystick input
*
* @author Matthias Grob <maetugr@gmail.com>
*/
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
class ManualControl
{
public:
ManualControl() = default;
~ManualControl() = default;
void update();
bool wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov,
const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
//private:
void process(manual_control_setpoint_s &manual_control_setpoint);
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
manual_control_setpoint_s _manual_control_setpoint{};
manual_control_setpoint_s _last_manual_control_setpoint{};
};