Replace old PID library with a new unit tested C++ class

src/lib/pid/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,4 +31,10 @@
 #
 ############################################################################
 
-px4_add_library(pid pid.cpp)
+px4_add_library(PID
+	PID.cpp
+	PID.hpp
+)
+target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)

src/lib/pid/PID.cpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "PID.hpp"
+#include "lib/mathlib/math/Functions.hpp"
+
+void PID::setGains(const float P, const float I, const float D)
+{
+	_gain_proportional = P;
+	_gain_integral = I;
+	_gain_derivative = D;
+}
+
+float PID::update(const float feedback, const float dt, const bool update_integral)
+{
+	const float error = _setpoint - feedback;
+	const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f;
+	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change);
+
+	if (update_integral) {
+		updateIntegral(error, dt);
+	}
+
+	_last_feedback = feedback;
+	return math::constrain(output, -_limit_output, _limit_output);
+}
+
+void PID::updateIntegral(float error, const float dt)
+{
+	const float integral_new = _integral + _gain_integral * error * dt;
+
+	if (std::isfinite(integral_new)) {
+		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
+	}
+}

src/lib/pid/PID.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+class PID
+{
+public:
+	PID() = default;
+	virtual ~PID() = default;
+	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setIntegralLimit(const float limit) { _limit_integral = limit; }
+	void setGains(const float P, const float I, const float D);
+	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	float update(const float feedback, const float dt, const bool update_integral = true);
+	float getIntegral() { return _integral; }
+	void resetIntegral() { _integral = 0.f; };
+	void resetDerivative() { _last_feedback = NAN; };
+private:
+	void updateIntegral(float error, const float dt);
+
+	float _setpoint{0.f}; ///< current setpoint to track
+	float _integral{0.f}; ///< integral state
+	float _last_feedback{NAN};
+
+	// Gains, Limits
+	float _gain_proportional{0.f};
+	float _gain_integral{0.f};
+	float _gain_derivative{0.f};
+	float _limit_integral{0.f};
+	float _limit_output{0.f};
+};

src/lib/pid/PIDTest.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PID.hpp>
+
+TEST(PIDTest, AllZeroCase)
+{
+	PID pid;
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+}
+
+TEST(PIDTest, OutputLimit)
+{
+	PID pid;
+	pid.setOutputLimit(.01f);
+	pid.setGains(.1f, 0.f, 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
+	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
+	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
+}
+
+TEST(PIDTest, ProportinalOnly)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(.1f, 0.f, 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+
+	float plant = 0.f;
+	float output = 10000.f;
+	int i; // need function scope to check how many steps
+
+	for (i = 1000; i > 0; i--) {
+		const float output_new = pid.update(plant, 0.f, false);
+		plant += output_new;
+
+		// expect the output to get smaller with each iteration
+		if (output_new >= output) {
+			break;
+		}
+
+		output = output_new;
+	}
+
+	EXPECT_FLOAT_EQ(plant, 1.f);
+	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
+}
+
+TEST(PIDTest, InteralOpenLoop)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Zero error
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+
+	// Open loop ramp up
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+
+	// Open loop ramp down
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	pid.resetIntegral();
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
+}