mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 10:46:33 +08:00
Commander: use parameters directly in ManualControl
This commit is contained in:
@@ -2096,11 +2096,9 @@ Commander::run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// abort autonomous mode and switch to position mode if sticks are moved significantly
|
// abort autonomous mode and switch to position mode if sticks are moved significantly
|
||||||
if ((_param_rc_override.get() != 0)
|
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
|
||||||
&& (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
|
|
||||||
&& !in_low_battery_failsafe && !_geofence_warning_action_on
|
&& !in_low_battery_failsafe && !_geofence_warning_action_on
|
||||||
&& _manual_control.wantsOverride(_param_rc_override.get(), _param_com_rc_stick_ov.get(), _vehicle_control_mode,
|
&& _manual_control.wantsOverride(_vehicle_control_mode, !_status.rc_signal_lost)) {
|
||||||
!_status.rc_signal_lost)) {
|
|
||||||
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
|
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
|
||||||
&_internal_state) == TRANSITION_CHANGED) {
|
&_internal_state) == TRANSITION_CHANGED) {
|
||||||
tune_positive(true);
|
tune_positive(true);
|
||||||
|
|||||||
@@ -245,10 +245,8 @@ private:
|
|||||||
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
|
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
|
||||||
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
|
(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_IN_MODE>) _param_rc_in_off,
|
||||||
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
|
(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_FLTMODE1>) _param_fltmode_1,
|
||||||
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
|
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
|
||||||
@@ -361,7 +359,7 @@ private:
|
|||||||
manual_control_setpoint_s _last_manual_control_setpoint{};
|
manual_control_setpoint_s _last_manual_control_setpoint{};
|
||||||
manual_control_switches_s _manual_control_switches{};
|
manual_control_switches_s _manual_control_switches{};
|
||||||
manual_control_switches_s _last_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
|
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] {};
|
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};
|
uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
|
||||||
|
|||||||
@@ -56,24 +56,23 @@ void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint)
|
|||||||
_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,
|
bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available)
|
||||||
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;
|
&& 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;
|
&& vehicle_control_mode.flag_control_offboard_enabled;
|
||||||
|
|
||||||
if (rc_available && (override_auto_mode || override_offboard_mode)) {
|
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)
|
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.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|
||||||
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
|
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
|
||||||
const bool throttle_moved =
|
const bool throttle_moved =
|
||||||
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
|
(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)) {
|
if (rpy_moved || (use_throttle && throttle_moved)) {
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
@@ -41,19 +41,20 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
|
||||||
class ManualControl
|
class ManualControl : ModuleParams
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
ManualControl() = default;
|
ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
|
||||||
~ManualControl() = default;
|
~ManualControl() override = default;
|
||||||
|
|
||||||
void update();
|
void update();
|
||||||
bool wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov,
|
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
|
||||||
const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
|
|
||||||
|
|
||||||
//private:
|
//private:
|
||||||
void process(manual_control_setpoint_s &manual_control_setpoint);
|
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)};
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
manual_control_setpoint_s _manual_control_setpoint{};
|
manual_control_setpoint_s _manual_control_setpoint{};
|
||||||
manual_control_setpoint_s _last_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
|
||||||
|
)
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user