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 @@
     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 @@
     float ts; // sample time in seconds
     float reading[maxStepwisePoints];
     float output[maxStepwisePoints];
+    float positiveHysteresis;
+    float negativeHysteresis;
 };
 
 float stepwise(const ec::StepwiseInfo& info, float value);
diff --git a/pid/stepwisecontroller.cpp b/pid/stepwisecontroller.cpp
index cea21e5..1e6c301 100644
--- a/pid/stepwisecontroller.cpp
+++ b/pid/stepwisecontroller.cpp
@@ -22,6 +22,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <cmath>
 #include <iostream>
 #include <map>
 #include <memory>
@@ -33,9 +34,28 @@
     // Get input value
     float input = input_proc();
 
-    // Calculate new output
-    float output = ec::stepwise(get_stepwise_info(), input);
+    ec::StepwiseInfo info = get_stepwise_info();
 
+    float output = lastOutput;
+
+    // Calculate new output if hysteresis allows
+    if (std::isnan(output))
+    {
+        output = ec::stepwise(info, input);
+        lastInput = input;
+    }
+    else if ((input - lastInput) > info.positiveHysteresis)
+    {
+        output = ec::stepwise(info, input);
+        lastInput = input;
+    }
+    else if ((lastInput - input) > info.negativeHysteresis)
+    {
+        output = ec::stepwise(info, input);
+        lastInput = input;
+    }
+
+    lastOutput = output;
     // Output new value
     output_proc(output);
 
diff --git a/pid/stepwisecontroller.hpp b/pid/stepwisecontroller.hpp
index c3af1a1..a6b7816 100644
--- a/pid/stepwisecontroller.hpp
+++ b/pid/stepwisecontroller.hpp
@@ -4,6 +4,7 @@
 #include "ec/stepwise.hpp"
 #include "fan.hpp"
 
+#include <limits>
 #include <memory>
 #include <vector>
 
@@ -49,4 +50,6 @@
     ec::StepwiseInfo _stepwise_info;
     std::string _id;
     std::vector<std::string> _inputs;
+    float lastInput = std::numeric_limits<float>::quiet_NaN();
+    float lastOutput = std::numeric_limits<float>::quiet_NaN();
 };