From 3b63a8a2c3d91ada4c14d4c8640c9617c4ef51ea Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 Nov 2019 13:31:40 +1100
Subject: [PATCH] AC_PID: added AC_PI controller

will be used by IMU heater
---
 libraries/AC_PID/AC_PI.cpp | 47 ++++++++++++++++++++++++++++++++++++++
 libraries/AC_PID/AC_PI.h   | 34 +++++++++++++++++++++++++++
 2 files changed, 81 insertions(+)
 create mode 100644 libraries/AC_PID/AC_PI.cpp
 create mode 100644 libraries/AC_PID/AC_PI.h

diff --git a/libraries/AC_PID/AC_PI.cpp b/libraries/AC_PID/AC_PI.cpp
new file mode 100644
index 0000000000..d3624eafb4
--- /dev/null
+++ b/libraries/AC_PID/AC_PI.cpp
@@ -0,0 +1,47 @@
+/*
+  Generic PI algorithm for controllers that don't need filtering (such as heaters)
+*/
+
+#include <AP_Math/AP_Math.h>
+#include "AC_PI.h"
+
+const AP_Param::GroupInfo AC_PI::var_info[] = {
+    // @Param: P
+    // @DisplayName: PID Proportional Gain
+    // @Description: P Gain which produces an output value that is proportional to the current error value
+    AP_GROUPINFO("P",    1, AC_PI, kP, 0),
+
+    // @Param: I
+    // @DisplayName: PID Integral Gain
+    // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
+    AP_GROUPINFO("I",    2, AC_PI, kI, 0),
+
+    // @Param: IMAX
+    // @DisplayName: PID Integral Maximum
+    // @Description: The maximum/minimum value that the I term can output
+    AP_GROUPINFO("IMAX", 3, AC_PI, imax, 0),
+
+    AP_GROUPEND
+};
+
+// Constructor
+AC_PI::AC_PI(float initial_p, float initial_i, float initial_imax)
+{
+    // load parameter values from eeprom
+    AP_Param::setup_object_defaults(this, var_info);
+
+    kP.set(initial_p);
+    kI.set(initial_i);
+    imax.set(initial_imax);
+}
+
+float AC_PI::update(float measurement, float target, float dt)
+{
+    const float err = target - measurement;
+
+    integrator += kI * err * dt;
+    integrator = constrain_float(integrator, 0, imax);
+    output_P = kP * err;
+
+    return output_P + integrator;
+}
diff --git a/libraries/AC_PID/AC_PI.h b/libraries/AC_PID/AC_PI.h
new file mode 100644
index 0000000000..f6586abf8f
--- /dev/null
+++ b/libraries/AC_PID/AC_PI.h
@@ -0,0 +1,34 @@
+#pragma once
+
+/*
+ Generic PI for systems like heater control, no filtering
+*/
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Param/AP_Param.h>
+
+class AC_PI {
+public:
+    // Constructor
+    AC_PI(float initial_p, float initial_i, float initial_imax);
+
+    // update controller
+    float update(float measurement, float target, float dt);
+
+    // parameter var table
+    static const struct AP_Param::GroupInfo var_info[];
+
+    float get_P() {
+        return output_P;
+    }
+    float get_I() {
+        return integrator;
+    }
+
+protected:
+    AP_Float        kP;
+    AP_Float        kI;
+    AP_Float        imax;
+    float           integrator;
+    float           output_P;
+};