From fb7c0376a07ed683178778def662d3b684aef3b1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 11 Feb 2019 11:57:05 -0500
Subject: [PATCH] systemlib hysteresis improve field packing and cleanup

---
 src/lib/systemlib/hysteresis/hysteresis.cpp | 31 ++++++++++++++-----
 src/lib/systemlib/hysteresis/hysteresis.h   | 34 +++++++--------------
 2 files changed, 35 insertions(+), 30 deletions(-)

diff --git a/src/lib/systemlib/hysteresis/hysteresis.cpp b/src/lib/systemlib/hysteresis/hysteresis.cpp
index 33aee834c1..473867d044 100644
--- a/src/lib/systemlib/hysteresis/hysteresis.cpp
+++ b/src/lib/systemlib/hysteresis/hysteresis.cpp
@@ -37,13 +37,21 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <px4_log.h>
-#include "systemlib/hysteresis/hysteresis.h"
-
+#include "hysteresis.h"
 
 namespace systemlib
 {
 
+void
+Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
+{
+	if (from_state == true) {
+		_time_from_true_us = new_hysteresis_time_us;
+
+	} else {
+		_time_from_false_us = new_hysteresis_time_us;
+	}
+}
 
 void
 Hysteresis::set_state_and_update(const bool new_state)
@@ -66,10 +74,19 @@ Hysteresis::update()
 {
 	if (_requested_state != _state) {
 
-		if (hrt_elapsed_time(&_last_time_to_change_state) >= (_state ?
-				_hysteresis_time_from_true_us :
-				_hysteresis_time_from_false_us)) {
-			_state = _requested_state;
+		const hrt_abstime elapsed = hrt_elapsed_time(&_last_time_to_change_state);
+
+		if (_state && !_requested_state) {
+			// true -> false
+			if (elapsed >= _time_from_true_us) {
+				_state = false;
+			}
+
+		} else if (!_state && _requested_state) {
+			// false -> true
+			if (elapsed >= _time_from_false_us) {
+				_state = true;
+			}
 		}
 	}
 }
diff --git a/src/lib/systemlib/hysteresis/hysteresis.h b/src/lib/systemlib/hysteresis/hysteresis.h
index 97cd6c049e..2fbc271d0c 100644
--- a/src/lib/systemlib/hysteresis/hysteresis.h
+++ b/src/lib/systemlib/hysteresis/hysteresis.h
@@ -49,31 +49,17 @@ namespace systemlib
 class Hysteresis
 {
 public:
-	Hysteresis(bool init_state) :
+	explicit 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)
+		_requested_state(init_state)
 	{}
+	Hysteresis() = delete; // no default constructor
 
-	~Hysteresis()
-	{}
+	~Hysteresis() = default;
 
-	void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
-	{
-		if (from_state == true) {
-			_hysteresis_time_from_true_us = new_hysteresis_time_us;
+	bool get_state() const { return _state; }
 
-		} else {
-			_hysteresis_time_from_false_us = new_hysteresis_time_us;
-		}
-	}
-
-	bool get_state() const
-	{
-		return _state;
-	}
+	void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us);
 
 	void set_state_and_update(const bool new_state);
 
@@ -81,11 +67,13 @@ public:
 
 private:
 
+	hrt_abstime _last_time_to_change_state{0};
+
+	hrt_abstime _time_from_true_us{0};
+	hrt_abstime _time_from_false_us{0};
+
 	bool _state;
 	bool _requested_state;
-	hrt_abstime _hysteresis_time_from_true_us;
-	hrt_abstime _hysteresis_time_from_false_us;
-	hrt_abstime _last_time_to_change_state;
 };
 
 } // namespace systemlib