Add hysteresis to stepwise controller

Tested-by: Ran on platform monitoring output and wrote
unit test

Change-Id: I74a1d21544c1a9cb4c1cb26dd4a353cbff0442d0
Signed-off-by: James Feist <james.feist@linux.intel.com>

2 files changed, 4 insertions, 1 deletions

diff --git a/pid/ec/stepwise.cpp b/pid/ec/stepwise.cpp
index 4246fb3..4a71532 100644
--- a/pid/ec/stepwise.cpp
+++ b/pid/ec/stepwise.cpp
@@ -16,6 +16,7 @@
 
 #include "stepwise.hpp"
 
+#include <cmath>
 #include <cstddef>
 #include <limits>
 
@@ -29,7 +30,7 @@ float stepwise(const ec::StepwiseInfo& info, float input)
     for (size_t ii = 1; ii < ec::maxStepwisePoints; ii++)
     {
 
-        if (info.reading[ii] == std::numeric_limits<float>::quiet_NaN())
+        if (std::isnan(info.reading[ii]))
         {
             break;
         }
diff --git a/pid/ec/stepwise.hpp b/pid/ec/stepwise.hpp
index ed07b44..4034b47 100644
--- a/pid/ec/stepwise.hpp
+++ b/pid/ec/stepwise.hpp
@@ -29,6 +29,8 @@ struct StepwiseInfo
     float ts; // sample time in seconds
     float reading[maxStepwisePoints];
     float output[maxStepwisePoints];
+    float positiveHysteresis;
+    float negativeHysteresis;
 };
 
 float stepwise(const ec::StepwiseInfo& info, float value);