diff --git a/pid/pidcontroller.cpp b/pid/pidcontroller.cpp
index fa8d6f6..b33037f 100644
--- a/pid/pidcontroller.cpp
+++ b/pid/pidcontroller.cpp
@@ -30,6 +30,81 @@
 namespace pid_control
 {
 
+double PIDController::calPIDOutput(double setpt, double input,
+                                   ec::pid_info_t* info)
+{
+    double output;
+    auto name = getID();
+
+    if (info->checkHysterWithSetpt)
+    {
+        // Over the hysteresis bounds, keep counting pid
+        if (input > (setpt + info->positiveHysteresis))
+        {
+            // Calculate new output
+            output = ec::pid(info, input, setpt, &name);
+
+            // this variable isn't actually used in this context, but we're
+            // setting it here incase somebody uses it later it's the correct
+            // value
+            lastInput = input;
+        }
+        // Under the hysteresis bounds, initialize pid
+        else if (input < (setpt - info->negativeHysteresis))
+        {
+            lastInput = setpt;
+            info->integral = 0;
+            output = 0;
+        }
+        // inside the hysteresis bounds, keep last output
+        else
+        {
+            lastInput = input;
+            output = info->lastOutput;
+        }
+
+        info->lastOutput = output;
+    }
+    else
+    {
+        // if no hysteresis, maintain previous behavior
+        if (info->positiveHysteresis == 0 && info->negativeHysteresis == 0)
+        {
+            // Calculate new output
+            output = ec::pid(info, input, setpt, &name);
+
+            // this variable isn't actually used in this context, but we're
+            // setting it here incase somebody uses it later it's the correct
+            // value
+            lastInput = input;
+        }
+        else
+        {
+            // initialize if the value is not set (NAN) or abnormal (+INF or
+            // -INF)
+            if (!(std::isfinite(lastInput)))
+            {
+                lastInput = input;
+            }
+
+            // if reading is outside of hysteresis bounds, use it for reading,
+            // otherwise use last reading without updating it first
+            else if ((input - lastInput) > info->positiveHysteresis)
+            {
+                lastInput = input;
+            }
+            else if ((lastInput - input) > info->negativeHysteresis)
+            {
+                lastInput = input;
+            }
+
+            output = ec::pid(info, lastInput, setpt, &name);
+        }
+    }
+
+    return output;
+}
+
 void PIDController::process(void)
 {
     double input;
@@ -43,39 +118,11 @@
     input = inputProc();
 
     auto info = getPIDInfo();
-    auto name = getID();
 
-    // if no hysteresis, maintain previous behavior
-    if (info->positiveHysteresis == 0 && info->negativeHysteresis == 0)
-    {
-        // Calculate new output
-        output = ec::pid(info, input, setpt, &name);
+    // Calculate output value
+    output = calPIDOutput(setpt, input, info);
 
-        // this variable isn't actually used in this context, but we're setting
-        // it here incase somebody uses it later it's the correct value
-        lastInput = input;
-    }
-    else
-    {
-        // initialize if not set yet
-        if (std::isnan(lastInput))
-        {
-            lastInput = input;
-        }
-
-        // if reading is outside of hysteresis bounds, use it for reading,
-        // otherwise use last reading without updating it first
-        else if ((input - lastInput) > info->positiveHysteresis)
-        {
-            lastInput = input;
-        }
-        else if ((lastInput - input) > info->negativeHysteresis)
-        {
-            lastInput = input;
-        }
-
-        output = ec::pid(info, lastInput, setpt, &name);
-    }
+    info->lastOutput = output;
 
     // Output new value
     outputProc(output);