mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Add new C++ PID library
This commit is contained in:
Matthias Grob
parent
e194a52907
commit
e047972cde
@ -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
|
||||
|
||||
65
src/lib/pid/PID.cpp
Normal file
65
src/lib/pid/PID.cpp
Normal file
@ -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);
|
||||
}
|
||||
}
|
||||
64
src/lib/pid/PID.hpp
Normal file
64
src/lib/pid/PID.hpp
Normal file
@ -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};
|
||||
};
|
||||
129
src/lib/pid/PIDTest.cpp
Normal file
129
src/lib/pid/PIDTest.cpp
Normal file
@ -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);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user