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: use parameters directly in ManualControl

Browse Source
This commit is contained in:
Matthias Grob 2021-02-16 08:45:33 +01:00
parent bfd830e580
commit 9ae1aee7ff
4 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2106,11 +2106,9 @@ 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)
if ((_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)) {
&& _manual_control.wantsOverride(_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);

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

@ -245,10 +245,8 @@ private:
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
@ -361,7 +359,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;
ManualControl _manual_control{this};
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};

11
src/modules/commander/ManualControl.cpp
View File

@ -56,24 +56,23 @@ void ManualControl::process(manual_control_setpoint_s &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)
bool ManualControl::wantsOverride(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)
const bool override_auto_mode = (_param_rc_override.get() & 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)
const bool override_offboard_mode = (_param_rc_override.get() & 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 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 & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
if (rpy_moved || (use_throttle && throttle_moved)) {
return true;

16
src/modules/commander/ManualControl.hpp
View File

@ -41,19 +41,20 @@
#pragma once
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
class ManualControl
class ManualControl : ModuleParams
{
public:
ManualControl() = default;
~ManualControl() = default;
ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
~ManualControl() override = 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);
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
//private:
void process(manual_control_setpoint_s &manual_control_setpoint);
@ -61,4 +62,9 @@ public:
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{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
)
};