From 2d1a044ebaedbc9bcf9e95d5b60420ed9e18a2d6 Mon Sep 17 00:00:00 2001
From: Raptor Engineering Development Team <support@raptorengineering.com>
Date: Sun, 29 Apr 2018 07:31:49 -0500
Subject: Add PID control option to fan controller

---
 control/zone.cpp | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 92 insertions(+)

(limited to 'control/zone.cpp')

diff --git a/control/zone.cpp b/control/zone.cpp
index 8e665f0..4ce62ea 100644
--- a/control/zone.cpp
+++ b/control/zone.cpp
@@ -13,6 +13,9 @@
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
+ *
+ * PID controls (c) 2018 Raptor Engineering, LLC
+ * Licensed for use with Raptor Computing Systems machines only
  */
 #include <chrono>
 #include <phosphor-logging/log.hpp>
@@ -49,6 +52,7 @@ Zone::Zone(Mode mode,
     _decInterval(std::get<decIntervalPos>(def)),
     _incTimer(events, [this](){ this->incTimerExpired(); }),
     _decTimer(events, [this](){ this->decTimerExpired(); }),
+    _pidTimer(events, [this](){ this->pidTimerExpired(); }),
     _sdEvents(events)
 {
     auto& fanDefs = std::get<fanListPos>(def);
@@ -77,6 +81,12 @@ Zone::Zone(Mode mode,
             _decTimer.start(_decInterval,
                             util::Timer::TimerType::repeating);
         }
+        // Start timer for PID loops
+        if (!_pidTimer.running())
+        {
+            _pidTimer.start(std::chrono::seconds(1),
+                            util::Timer::TimerType::repeating);
+        }
     }
 }
 
@@ -102,6 +112,9 @@ void Zone::setFullSpeed()
             fan->setSpeed(_targetSpeed);
         }
     }
+
+    _pidIntegrator = 0;
+    _pidPrevError = 0;
 }
 
 void Zone::disableRotors()
@@ -110,6 +123,9 @@ void Zone::disableRotors()
     {
         fan->disableRotor();
     }
+
+    _pidIntegrator = 0;
+    _pidPrevError = 0;
 }
 
 void Zone::setActiveAllow(const Group* group, bool isActiveAllow)
@@ -127,6 +143,9 @@ void Zone::setActiveAllow(const Group* group, bool isActiveAllow)
                                 _active.end(),
                                 actPred);
     }
+
+    _pidIntegrator = 0;
+    _pidPrevError = 0;
 }
 
 void Zone::removeService(const Group* group,
@@ -275,6 +294,16 @@ void Zone::requestSpeedDecrease(uint64_t targetDelta)
     }
 }
 
+void Zone::runPidLoop(int64_t error, uint64_t integrator_timestep, int64_t kp, int64_t ki, int64_t kd)
+{
+    _pidLastError = error;
+    _pidLastIntegratorTimestep = integrator_timestep;
+    _pidLastKp = kp;
+    _pidLastKi = ki;
+    _pidLastKd = kd;
+    _pidActive = true;
+}
+
 void Zone::decTimerExpired()
 {
     // Check all entries are set to allow a decrease
@@ -312,6 +341,67 @@ void Zone::decTimerExpired()
     // Decrease timer is restarted since its repeating
 }
 
+void Zone::pidTimerExpired()
+{
+    if (!_pidActive)
+    {
+        return;
+    }
+
+    /* If error is positive on startup, set fans to full by forcing integrator to maximum value */
+    if (!_pidActivePrev) {
+        if (_pidLastError > 0) {
+            _pidIntegrator = ((_ceilingSpeed - _floorSpeed) * 100000) / _pidLastKi;
+        }
+    }
+
+    auto pidIntegrator_orig = _pidIntegrator;
+    _pidIntegrator += _pidLastError * _pidLastIntegratorTimestep;
+    int64_t derivative = (_pidLastError - _pidPrevError) / _pidLastIntegratorTimestep;
+    int64_t pid_output = ((_pidLastError * _pidLastKp) + (_pidIntegrator * _pidLastKi) + (derivative * _pidLastKd)) / 100000;
+    _pidPrevError = _pidLastError;
+
+    uint64_t requestTarget = _floorSpeed;
+    if (pid_output > 0)
+    {
+        requestTarget = _floorSpeed + pid_output;
+    }
+    else {
+        _pidIntegrator = pidIntegrator_orig;
+    }
+    // Target speed can not go above a defined ceiling speed
+    if (requestTarget > _ceilingSpeed)
+    {
+        requestTarget = _ceilingSpeed;
+        _pidIntegrator = pidIntegrator_orig;
+    }
+    // Target speed can not go below a defined floor speed
+    if (requestTarget < _floorSpeed)
+    {
+        requestTarget = _floorSpeed;
+        _pidIntegrator = pidIntegrator_orig;
+    }
+
+    /* Cap integrator wind-up */
+    if (_pidLastKi != 0) {
+        if (_pidIntegrator > (int64_t)(((_ceilingSpeed - _floorSpeed) * 100000) / _pidLastKi))
+        {
+            _pidIntegrator = ((_ceilingSpeed - _floorSpeed) * 100000) / _pidLastKi;
+        }
+        if (_pidIntegrator < ((int64_t)((_ceilingSpeed - _floorSpeed) * -100000) / _pidLastKi))
+        {
+            _pidIntegrator = ((_ceilingSpeed - _floorSpeed) * -100000) / _pidLastKi;
+        }
+    }
+    else {
+        _pidIntegrator = 0;
+    }
+
+    setRequestSpeedBase(requestTarget);
+
+    _pidActivePrev = true;
+}
+
 void Zone::initEvent(const SetSpeedEvent& event)
 {
     sdbusplus::message::message nullMsg{nullptr};
@@ -319,6 +409,8 @@ void Zone::initEvent(const SetSpeedEvent& event)
     for (auto& sig : std::get<signalsPos>(event))
     {
         // Initialize the event signal using handler
+        // FIXME
+        // This next line is very slow, delaying fan control startup significantly
         std::get<sigHandlerPos>(sig)(_bus, nullMsg, *this);
         // Setup signal matches of the property for event
         std::unique_ptr<EventData> eventData =
-- 
cgit v1.2.1