From e047972cde2bbd4678a0a25ffc701dde7eedaac8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 17:31:28 +0100 Subject: [PATCH] Add new C++ PID library --- src/lib/pid/CMakeLists.txt | 12 +++- src/lib/pid/PID.cpp | 65 +++++++++++++++++++ src/lib/pid/PID.hpp | 64 ++++++++++++++++++ src/lib/pid/PIDTest.cpp | 129 +++++++++++++++++++++++++++++++++++++ 4 files changed, 268 insertions(+), 2 deletions(-) create mode 100644 src/lib/pid/PID.cpp create mode 100644 src/lib/pid/PID.hpp create mode 100644 src/lib/pid/PIDTest.cpp diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index b8ed30b7a2..044e1b39b4 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/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,12 @@ # ############################################################################ -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) + +px4_add_library(pid pid.cpp) # TODO: remove deprecated pid library diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp new file mode 100644 index 0000000000..3f0d9b4b93 --- /dev/null +++ b/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); + } +} diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp new file mode 100644 index 0000000000..cabdd3701b --- /dev/null +++ b/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 + +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}; +}; diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp new file mode 100644 index 0000000000..ed3c204e6e --- /dev/null +++ b/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 +#include + +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); +}