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>
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);