hysteresis: we needed different hysteresis

Sometimes, we only need a histeresis in one direction.

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;
+		}
 	}
 }
 

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

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());
 }