diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7ba1c2a327..1849e368bd 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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() _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() _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 diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index be17bb6237..e57f05c0e3 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -235,7 +235,6 @@ private: (ParamFloat) _param_ef_time_thres, (ParamBool) _param_arm_without_gps, - (ParamBool) _param_arm_switch_is_button, (ParamBool) _param_arm_mission_required, (ParamBool) _param_arm_auth_required, (ParamBool) _param_escs_checks_required, @@ -245,7 +244,6 @@ private: (ParamInt) _param_takeoff_finished_action, (ParamInt) _param_rc_in_off, - (ParamInt) _param_rc_arm_hyst, (ParamInt) _param_fltmode_1, (ParamInt) _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}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index e7a57ff8be..964b1d7304 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -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); +} diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index 3338515270..5edec1c3c0 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -41,11 +41,14 @@ #pragma once +#include #include #include #include +#include #include +#include 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) _param_com_rc_loss_t, + (ParamInt) _param_rc_arm_hyst, + (ParamBool) _param_arm_switch_is_button, + (ParamBool) _param_com_rearm_grace, (ParamInt) _param_rc_override, (ParamFloat) _param_com_rc_stick_ov )