diff --git a/dbus/dbusconfiguration.cpp b/dbus/dbusconfiguration.cpp
index b912e62..0443854 100644
--- a/dbus/dbusconfiguration.cpp
+++ b/dbus/dbusconfiguration.cpp
@@ -444,6 +444,22 @@
                         base.at("Name"))];
                 info.type = "stepwise";
                 info.stepwiseInfo.ts = 1.0; // currently unused
+                info.stepwiseInfo.positiveHysteresis = 0.0;
+                info.stepwiseInfo.negativeHysteresis = 0.0;
+                auto findPosHyst = base.find("PositiveHysteresis");
+                auto findNegHyst = base.find("NegativeHysteresis");
+                if (findPosHyst != base.end())
+                {
+                    info.stepwiseInfo.positiveHysteresis =
+                        mapbox::util::apply_visitor(VariantToFloatVisitor(),
+                                                    findPosHyst->second);
+                }
+                if (findNegHyst != base.end())
+                {
+                    info.stepwiseInfo.positiveHysteresis =
+                        mapbox::util::apply_visitor(VariantToFloatVisitor(),
+                                                    findNegHyst->second);
+                }
                 std::vector<double> readings =
                     sdbusplus::message::variant_ns::get<std::vector<double>>(
                         base.at("Reading"));
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();
 };
diff --git a/test/Makefile.am b/test/Makefile.am
index 4a01994..31e6142 100644
--- a/test/Makefile.am
+++ b/test/Makefile.am
@@ -14,6 +14,7 @@
 check_PROGRAMS = sensor_manager_unittest sensor_pluggable_unittest \
  sensor_host_unittest util_unittest pid_zone_unittest \
  pid_thermalcontroller_unittest pid_fancontroller_unittest \
+ pid_stepwisecontroller_unittest \
  dbus_passive_unittest dbus_active_unittest
 TESTS = $(check_PROGRAMS)
 
@@ -40,6 +41,10 @@
  $(top_builddir)/pid/util.o $(top_builddir)/pid/pidcontroller.o \
  $(top_builddir)/pid/thermalcontroller.o
 
+pid_stepwisecontroller_unittest_SOURCES = pid_stepwisecontroller_unittest.cpp
+pid_stepwisecontroller_unittest_LDADD = $(top_builddir)/pid/ec/stepwise.o \
+ $(top_builddir)/pid/util.o $(top_builddir)/pid/stepwisecontroller.o
+
 pid_fancontroller_unittest_SOURCES = pid_fancontroller_unittest.cpp
 pid_fancontroller_unittest_LDADD = $(top_builddir)/pid/ec/pid.o \
  $(top_builddir)/pid/util.o $(top_builddir)/pid/pidcontroller.o \
diff --git a/test/pid_stepwisecontroller_unittest.cpp b/test/pid_stepwisecontroller_unittest.cpp
new file mode 100644
index 0000000..45d1b81
--- /dev/null
+++ b/test/pid_stepwisecontroller_unittest.cpp
@@ -0,0 +1,83 @@
+#include "pid/controller.hpp"
+#include "pid/ec/stepwise.hpp"
+#include "pid/stepwisecontroller.hpp"
+#include "test/zone_mock.hpp"
+
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+using ::testing::Return;
+using ::testing::StrEq;
+
+TEST(StepwiseControllerTest, HysteresisTestPositive)
+{
+    // Verifies positive hysteresis works as expected
+
+    ZoneMock z;
+
+    std::vector<std::string> inputs = {"test"};
+    ec::StepwiseInfo initial;
+    initial.negativeHysteresis = 3.0;
+    initial.positiveHysteresis = 2.0;
+    initial.reading[0] = 20.0;
+    initial.reading[1] = 30.0;
+    initial.reading[2] = std::numeric_limits<float>::quiet_NaN();
+    initial.output[0] = 40.0;
+    initial.output[1] = 60.0;
+
+    std::unique_ptr<Controller> p =
+        StepwiseController::CreateStepwiseController(&z, "foo", inputs,
+                                                     initial);
+
+    EXPECT_CALL(z, getCachedValue(StrEq("test")))
+        .Times(3)
+        .WillOnce(Return(29.0))  // return 40
+        .WillOnce(Return(31.0))  // return 40
+        .WillOnce(Return(32.0)); // return 60
+
+    EXPECT_CALL(z, addRPMSetPoint(40.0)).Times(2);
+    EXPECT_CALL(z, addRPMSetPoint(60.0)).Times(1);
+
+    for (int ii = 0; ii < 3; ii++)
+    {
+        p->process();
+    }
+}
+
+TEST(StepwiseControllerTest, HysteresisTestNegative)
+{
+    // Verifies negative hysteresis works as expected
+
+    ZoneMock z;
+
+    std::vector<std::string> inputs = {"test"};
+    ec::StepwiseInfo initial;
+    initial.negativeHysteresis = 3.0;
+    initial.positiveHysteresis = 2.0;
+    initial.reading[0] = 20.0;
+    initial.reading[1] = 30.0;
+    initial.reading[2] = std::numeric_limits<float>::quiet_NaN();
+    initial.output[0] = 40.0;
+    initial.output[1] = 60.0;
+
+    std::unique_ptr<Controller> p =
+        StepwiseController::CreateStepwiseController(&z, "foo", inputs,
+                                                     initial);
+
+    EXPECT_CALL(z, getCachedValue(StrEq("test")))
+        .Times(3)
+        .WillOnce(Return(30.0))  // return 60
+        .WillOnce(Return(27.0))  // return 60
+        .WillOnce(Return(26.0)); // return 40
+
+    EXPECT_CALL(z, addRPMSetPoint(40.0)).Times(1);
+    EXPECT_CALL(z, addRPMSetPoint(60.0)).Times(2);
+
+    for (int ii = 0; ii < 3; ii++)
+    {
+        p->process();
+    }
+}