From f365832c0bdaa063cf496e14f86e51e0accc4aa8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:03:51 +0100
Subject: [PATCH] hysteresis: we needed different hysteresis

Sometimes, we only need a histeresis in one direction.
---
 .../systemlib/hysteresis/hysteresis.cpp       | 11 ++--
 src/modules/systemlib/hysteresis/hysteresis.h | 17 ++++--
 unittests/hysteresis_test.cpp                 | 57 ++++++++++++++++---
 3 files changed, 67 insertions(+), 18 deletions(-)

diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp
index a04c21883c..0b4e86ed9e 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.cpp
+++ b/src/modules/systemlib/hysteresis/hysteresis.cpp
@@ -61,12 +61,13 @@ Hysteresis::set_state_and_update(const bool new_state)
 void
 Hysteresis::update()
 {
+	if (_requested_state != _state) {
 
-
-	if (_requested_state != _state &&
-	    hrt_elapsed_time(&_last_time_to_change_state) >= _hysteresis_time_us) {
-
-		_state = _requested_state;
+		if (hrt_elapsed_time(&_last_time_to_change_state) >= (_state ?
+								      _hysteresis_time_from_true_us :
+								      _hysteresis_time_from_false_us)) {
+			_state = _requested_state;
+		}
 	}
 }
 
diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h
index ba841071e8..a151ee85dd 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.h
+++ b/src/modules/systemlib/hysteresis/hysteresis.h
@@ -48,16 +48,25 @@ namespace systemlib {
 class Hysteresis
 {
 public:
-	Hysteresis(unsigned hysteresis_time_us, bool init_state) :
-		_hysteresis_time_us(hysteresis_time_us),
+	Hysteresis(bool init_state) :
 		_state(init_state),
 		_requested_state(init_state),
+		_hysteresis_time_from_true_us(0),
+		_hysteresis_time_from_false_us(0),
 		_last_time_to_change_state(0)
 	{}
 
 	~Hysteresis()
 	{}
 
+	void set_hysteresis_time_from(const bool from_state, const unsigned new_hysteresis_time_us) {
+		if (from_state == true) {
+			_hysteresis_time_from_true_us = new_hysteresis_time_us;
+		} else {
+			_hysteresis_time_from_false_us = new_hysteresis_time_us;
+		}
+	}
+
 	bool get_state() const {
 		return _state;
 	}
@@ -68,10 +77,10 @@ public:
 
 private:
 
-	unsigned _hysteresis_time_us;
 	bool _state;
-
 	bool _requested_state;
+	unsigned _hysteresis_time_from_true_us;
+	unsigned _hysteresis_time_from_false_us;
 	hrt_abstime _last_time_to_change_state;
 };
 
diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp
index a01fc3f2e2..99be56f317 100644
--- a/unittests/hysteresis_test.cpp
+++ b/unittests/hysteresis_test.cpp
@@ -2,23 +2,23 @@
 
 #include "gtest/gtest.h"
 
-const static unsigned hysteresis_time_us = 5000;
 
 TEST(HysteresisTest, InitFalse)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
 	ASSERT_FALSE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, InitTrue)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, true);
+	systemlib::Hysteresis hysteresis(true);
 	ASSERT_TRUE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, ZeroCase)
 {
-	systemlib::Hysteresis hysteresis(0, false);
+	// Default is 0 hysteresis.
+	systemlib::Hysteresis hysteresis(false);
 	ASSERT_FALSE(hysteresis.get_state());
 
 	// Change and see result immediately.
@@ -37,7 +37,36 @@ TEST(HysteresisTest, ZeroCase)
 
 TEST(HysteresisTest, ChangeAfterTime)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+	hysteresis.set_hysteresis_time_from(true, 3000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(4000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(2000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(1000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, HysteresisChanged)
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 2000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -49,20 +78,25 @@ TEST(HysteresisTest, ChangeAfterTime)
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
 
+	// Change hysteresis time.
+	hysteresis.set_hysteresis_time_from(true, 10000);
+
 	// Change back to false.
 	hysteresis.set_state_and_update(false);
 	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
+	usleep(7000);
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
+	usleep(5000);
 	hysteresis.update();
 	ASSERT_FALSE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 5000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -87,7 +121,8 @@ TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
 
 TEST(HysteresisTest, TakeChangeBack)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -111,4 +146,8 @@ TEST(HysteresisTest, TakeChangeBack)
 	usleep(3000);
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
+
+	// The other directory is immediate.
+	hysteresis.set_state_and_update(false);
+	ASSERT_FALSE(hysteresis.get_state());
 }