dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ManualControl: introduce stick gesture for killing

This commit is contained in:
Matthias Grob 2021-10-21 18:27:08 +02:00 committed by Mathieu Bresciani
parent 2110eae25d
commit 78987f6016
4 changed files with 45 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;

24
src/modules/manual_control/ManualControl.cpp
View File

@ -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)

4
src/modules/manual_control/ManualControl.hpp
View File

@ -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<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
(ParamBool<px4::params::MAN_ARM_GESTURE>) _param_man_arm_gesture,
(ParamFloat<px4::params::MAN_KILL_GEST_T>) _param_man_kill_gest_t,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_com_rc_arm_hyst,
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,

17
src/modules/manual_control/manual_control_params.c
View File

@ -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);