diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d3ac4c1776..c0c4e17b89 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1615,9 +1615,9 @@ void Commander::executeActionRequest(const action_request_s &action_request) break; case action_request_s::ACTION_UNKILL: - if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _actuator_armed.manual_lockdown) { - mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t"); - events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged"); + if (_actuator_armed.manual_lockdown) { + mavlink_log_info(&_mavlink_log_pub, "Kill disengaged\t"); + events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill disengaged"); _status_changed = true; _actuator_armed.manual_lockdown = false; } @@ -1625,8 +1625,8 @@ void Commander::executeActionRequest(const action_request_s &action_request) break; case action_request_s::ACTION_KILL: - if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_actuator_armed.manual_lockdown) { - const char kill_switch_string[] = "Kill-switch engaged\t"; + if (!_actuator_armed.manual_lockdown) { + const char kill_switch_string[] = "Kill engaged\t"; events::LogLevels log_levels{events::Log::Info}; if (_vehicle_land_detected.landed) { @@ -1637,7 +1637,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) log_levels.external = events::Log::Critical; } - events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill-switch engaged"); + events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill engaged"); _status_changed = true; _actuator_armed.manual_lockdown = true; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index ac549d6376..ecdcade3d4 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -154,7 +154,8 @@ void ManualControl::processInput(hrt_abstime now) _throttle_diff.reset(); _stick_arm_hysteresis.set_state_and_update(false, now); _stick_disarm_hysteresis.set_state_and_update(false, now); - _button_hysteresis.set_state_and_update(false, now); + _stick_kill_hysteresis.set_state_and_update(false, now); + _button_arm_hysteresis.set_state_and_update(false, now); } processSwitches(now); @@ -178,10 +179,10 @@ void ManualControl::processSwitches(hrt_abstime &now) if (_param_com_arm_swisbtn.get()) { // Arming button - const bool previous_button_hysteresis = _button_hysteresis.get_state(); - _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); + const bool previous_button_arm_hysteresis = _button_arm_hysteresis.get_state(); + _button_arm_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); - if (!previous_button_hysteresis && _button_hysteresis.get_state()) { + if (!previous_button_arm_hysteresis && _button_arm_hysteresis.get_state()) { sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_request_s::SOURCE_RC_BUTTON); } @@ -291,7 +292,8 @@ void ManualControl::updateParams() _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); - _button_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); + _button_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); + _stick_kill_hysteresis.set_hysteresis_time_from(false, _param_man_kill_gest_t.get() * 1_s); _selector.setRcInMode(_param_com_rc_in_mode.get()); _selector.setTimeout(_param_com_rc_loss_t.get() * 1_s); @@ -366,6 +368,18 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input) if (_param_man_arm_gesture.get() && !previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) { sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE); } + + // Kill gesture + if (_param_man_kill_gest_t.get() > 0.f) { + const bool right_stick_lower_right = (input.pitch < -0.9f) && (input.roll > 0.9f); + + const bool previous_stick_kill_hysteresis = _stick_kill_hysteresis.get_state(); + _stick_kill_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_lower_right, input.timestamp); + + if (!previous_stick_kill_hysteresis && _stick_kill_hysteresis.get_state()) { + sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_STICK_GESTURE); + } + } } void ManualControl::evaluateModeSlot(uint8_t mode_slot) diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 8e8bce13a5..660a125665 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -122,7 +122,8 @@ private: systemlib::Hysteresis _stick_arm_hysteresis{false}; systemlib::Hysteresis _stick_disarm_hysteresis{false}; - systemlib::Hysteresis _button_hysteresis{false}; + systemlib::Hysteresis _stick_kill_hysteresis{false}; + systemlib::Hysteresis _button_arm_hysteresis{false}; MovingDiff _roll_diff{}; MovingDiff _pitch_diff{}; @@ -146,6 +147,7 @@ private: (ParamFloat) _param_com_rc_loss_t, (ParamFloat) _param_com_rc_stick_ov, (ParamBool) _param_man_arm_gesture, + (ParamFloat) _param_man_kill_gest_t, (ParamInt) _param_com_rc_arm_hyst, (ParamBool) _param_com_arm_swisbtn, (ParamInt) _param_fltmode_1, diff --git a/src/modules/manual_control/manual_control_params.c b/src/modules/manual_control/manual_control_params.c index c17dc15ad7..7fde0d6dd3 100644 --- a/src/modules/manual_control/manual_control_params.c +++ b/src/modules/manual_control/manual_control_params.c @@ -41,3 +41,20 @@ * @group Manual Control */ PARAM_DEFINE_INT32(MAN_ARM_GESTURE, 1); + +/** + * Trigger time for kill stick gesture + * + * The timeout for holding the left stick to the lower left + * and the right stick to the lower right at the same time until the gesture + * kills the actuators one-way. + * + * A negative value disables the feature. + * + * @group Manual Control + * @unit s + * @decimal 2 + * @min -1 + * @max 15 + */ +PARAM_DEFINE_FLOAT(MAN_KILL_GEST_T, -1.f);