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: move rc arming to ManualControl class

Browse Source 
Separating the different arming methods is the next step.
This commit is contained in:
Matthias Grob 2021-02-17 14:21:49 +01:00
parent ce2d4c5336
commit 1938d366d9
4 changed files with 118 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1556,9 +1556,6 @@ Commander::run()
_last_gpos_fail_time_us = _boot_timestamp;
_last_lvel_fail_time_us = _boot_timestamp;
// user adjustable duration required to assert arm/disarm via throttle/rudder stick
uint32_t rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
int32_t airmode = 0;
int32_t rc_map_arm_switch = 0;
@ -1633,8 +1630,6 @@ Commander::run()
_status.rc_input_mode = _param_rc_in_off.get();
rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
_arm_requirements.arm_authorization = _param_arm_auth_required.get();
_arm_requirements.esc_check = _param_escs_checks_required.get();
_arm_requirements.global_position = !_param_arm_without_gps.get();
@ -2159,102 +2154,29 @@ Commander::run()
_status.rc_signal_lost = false;
const bool in_armed_state = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped =
_manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
/* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
* and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
* do it only for rotary wings in manual mode or fixed wing if landed.
* Disable stick-disarming if arming switch or button is mapped */
const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
&& (_manual_control_setpoint.z < 0.1f)
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
(_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);
if (in_armed_state &&
(_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) &&
(stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_climb_rate_enabled;
const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
|| arm_switch_to_disarm_transition;
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
if (arm_switch_to_disarm_transition) {
disarm(arm_disarm_reason_t::RC_SWITCH);
} else {
disarm(arm_disarm_reason_t::RC_STICK);
}
}
_stick_off_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0;
if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
disarm(arm_disarm_reason_t::RC_STICK);
}
/* ARM
* check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
* and we're in MANUAL mode.
* Disable stick-arming if arming switch or button is mapped */
const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
&& !arm_switch_or_button_mapped;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidental in-air disarming */
const bool in_rearming_grace_period = _param_com_rearm_grace.get() && (_last_disarmed_timestamp != 0)
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
arm(arm_disarm_reason_t::RC_STICK);
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
(_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) &&
(_manual_control_setpoint.z < 0.1f || in_rearming_grace_period);
if (!in_armed_state &&
(_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
(stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
if ((_stick_on_counter == rc_arm_hyst && _stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
/* we check outside of the transition function here because the requirement
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if (!_vehicle_control_mode.flag_control_manual_enabled) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first");
tune_negative(true);
} else {
if (arm_switch_to_arm_transition) {
arm(arm_disarm_reason_t::RC_SWITCH);
} else {
arm(arm_disarm_reason_t::RC_STICK);
}
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
tune_negative(true);
}
_stick_on_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0;
}
_last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch;
if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
// handle landing gear switch if configured and in a manual mode

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

@ -235,7 +235,6 @@ private:
(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_arm_switch_is_button,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
@ -245,7 +244,6 @@ private:
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
(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_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
@ -285,9 +283,6 @@ private:
/* 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)};
static constexpr float STICK_ON_OFF_LIMIT{0.9f};
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms};
@ -360,9 +355,6 @@ private:
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};
uint32_t _stick_off_counter{0};
uint32_t _stick_on_counter{0};
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};

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

@ -88,3 +88,96 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
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 in_armed_state = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped =
manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
const bool stick_in_lower_left = _manual_control_setpoint.r < -.9f
&& (_manual_control_setpoint.z < .1f)
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get()
&& (_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);
if (in_armed_state
&& (vehicle_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF)
&& (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || landed)
&& (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
const bool manual_thrust_mode = vehicle_control_mode.flag_control_manual_enabled
&& !vehicle_control_mode.flag_control_climb_rate_enabled;
bool disarm_trigger = _stick_disarm_hysteresis.get_state();
_stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time());
disarm_trigger = !disarm_trigger && _stick_disarm_hysteresis.get_state();
const bool rc_wants_disarm = (disarm_trigger) || arm_switch_to_disarm_transition;
if (rc_wants_disarm && (landed || manual_thrust_mode)) {
ret = true;
}
} else if (!(_param_arm_switch_is_button.get()
&& manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
_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,
manual_control_switches_s &manual_control_switches, const bool landed)
{
bool ret = false;
const bool in_armed_state = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped =
manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
const bool stick_in_lower_right = _manual_control_setpoint.r > .9f
&& _manual_control_setpoint.z < 0.1f
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get()
&& (_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 (!in_armed_state
&& (vehicle_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF)
&& (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
bool arm_trigger = _stick_disarm_hysteresis.get_state();
_stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time());
arm_trigger = !arm_trigger && _stick_arm_hysteresis.get_state();
if (arm_trigger || arm_switch_to_arm_transition) {
ret = true;
}
} else if (!(_param_arm_switch_is_button.get()
&& manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
_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);
}

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

@ -41,11 +41,14 @@
#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
{
@ -57,20 +60,34 @@ public:
void update();
bool isRCAvailable() { return _rc_available; }
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,
manual_control_switches_s &manual_control_switches, const bool landed);
manual_control_setpoint_s _manual_control_setpoint{};
private:
void updateParams() override;
void process(manual_control_setpoint_s &manual_control_setpoint);
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(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_arm_switch_is_button,
(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
)