dsp: add PID controller

common/include/villas/dsp/pid.hpp

@@ -0,0 +1,57 @@
+/** A PID controller.
+ *
+ * @file
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC
+ * @license GNU General Public License (version 3)
+ *
+ * VILLAScommon
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *********************************************************************************/
+
+#pragma once
+
+namespace villas {
+namespace dsp {
+
+class PID {
+
+protected:
+	double dt;
+        double max;
+        double min;
+        double Kp;
+        double Kd;
+        double Ki;
+        double pre_error;
+        double integral;
+
+public:
+        /**
+	 *  Kp -  proportional gain
+         * Ki -  Integral gain
+         * Kd -  derivative gain
+         * dt -  loop interval time
+         * max - maximum value of manipulated variable
+         * min - minimum value of manipulated variable
+	 */
+        PID(double _dt, double _max, double _min, double _Kp, double _Kd, double _Ki);
+
+        /** Returns the manipulated variable given a setpoint and current process value */
+        double calculate(double setpoint, double pv);
+};
+
+} // namespace dsp
+} // namespace villas

common/lib/CMakeLists.txt

@@ -27,6 +27,7 @@ add_library(villas-common SHARED
 	compat.cpp
 	crypt.cpp
 	hist.cpp
+	dsp/pid.cpp
 	kernel/kernel.cpp
 	kernel/kernel.cpp
 	kernel/rt.cpp

common/lib/dsp/pid.cpp

@@ -0,0 +1,68 @@
+/** A PID controller.
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC
+ * @license GNU General Public License (version 3)
+ *
+ * VILLAScommon
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *********************************************************************************/
+
+#include <iostream>
+#include <cmath>
+#include <villas/dsp/pid.hpp>
+
+using namespace std;
+using namespace villas::dsp;
+
+PID::PID(double _dt, double _max, double _min, double _Kp, double _Kd, double _Ki) :
+	dt(_dt),
+	max(_max),
+	min(_min),
+	Kp(_Kp),
+	Kd(_Kd),
+	Ki(_Ki)
+{ }
+
+double PID::calculate(double setpoint, double pv)
+{
+	/* Calculate error */
+	double error = setpoint - pv;
+
+	/* Proportional term */
+	double Pout = Kp * error;
+
+	/* Integral term */
+	integral += error * dt;
+	double Iout = Ki * integral;
+
+	/* Derivative term */
+	double derivative = (error - pre_error) / dt;
+	double Dout = Kd * derivative;
+
+	/* Calculate total output */
+	double output = Pout + Iout + Dout;
+
+	/* Restrict to max/min */
+	if (output > max)
+		output = max;
+	else if (output < min)
+		output = min;
+
+	/* Save error to previous error */
+	pre_error = error;
+
+	return output;
+}