From 60de330f184839c28836456a1ad9844c688f93f9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 16 Jun 2021 09:09:08 +0200
Subject: [PATCH] manual_control: add hysteresis for arm button back

---
 src/modules/manual_control/ManualControl.cpp | 5 ++++-
 src/modules/manual_control/ManualControl.hpp | 1 +
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp
index 5366c67020..0212304a96 100644
--- a/src/modules/manual_control/ManualControl.cpp
+++ b/src/modules/manual_control/ManualControl.cpp
@@ -82,6 +82,7 @@ void ManualControl::Run()
 
 		_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
 		_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
+		_button_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
 
 		_selector.set_rc_in_mode(_param_com_rc_in_mode.get());
 		_selector.set_timeout(_param_com_rc_loss_t.get() * 1_s);
@@ -176,7 +177,9 @@ void ManualControl::Run()
 					if (switches.arm_switch != _previous_switches.arm_switch) {
 						if (_param_com_arm_swisbtn.get()) {
 							// Arming button
-							if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
+							_button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now);
+
+							if (_button_hysteresis.get_state()) {
 								send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON);
 							}
 
diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp
index 410cede1ad..affe480b1d 100644
--- a/src/modules/manual_control/ManualControl.hpp
+++ b/src/modules/manual_control/ManualControl.hpp
@@ -152,6 +152,7 @@ private:
 
 	systemlib::Hysteresis _stick_arm_hysteresis{false};
 	systemlib::Hysteresis _stick_disarm_hysteresis{false};
+	systemlib::Hysteresis _button_hysteresis{false};
 
 	ManualControlSelector _selector;
 	bool _published_invalid_once{false};