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