zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander: move rc arming to ManualControl class 

Separating the different arming methods is the next step.
release/1.12
Matthias Grob 4 years ago
parent
00a4133042
commit
37ea78a7ff
4 changed files with 118 additions and 94 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 94
      src/modules/commander/Commander.cpp
  2. 8
      src/modules/commander/Commander.hpp
  3. 93
      src/modules/commander/ManualControl.cpp
  4. 17
      src/modules/commander/ManualControl.hpp

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

@ -1546,9 +1546,6 @@ Commander::run() @@ -1546,9 +1546,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;
@ -1623,8 +1620,6 @@ Commander::run() @@ -1623,8 +1620,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();
@ -2149,102 +2144,29 @@ Commander::run() @@ -2149,102 +2144,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);
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);
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);
} 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
Unescape View File

@ -235,7 +235,6 @@ private: @@ -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: @@ -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: @@ -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: @@ -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
Unescape View File

@ -88,3 +88,96 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_ @@ -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_arm_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
Unescape View File

@ -41,11 +41,14 @@ @@ -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: @@ -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
)

Write Preview
Loading…
Cancel
Save