rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-12 02:54:15 +01:00
Code Issues Releases Activity

LP-289 cleaned up PIDControlNE::UpdateParameters()

Browse Source
This commit is contained in:
paul Jewell 2016-12-10 14:07:11 +01:00
parent c723a06e93
commit a19bd40259
1 changed files with 32 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
flight/modules/PathFollower/pidcontrolne.cpp
View File

@ -7,7 +7,8 @@
* @{ * @{
* *
* @file PIDControlNE.h * @file PIDControlNE.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control loops for NE directions * @brief Executes PID control loops for NE directions
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
@ -79,26 +80,38 @@ void PIDControlNE::Activate()
void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{ {
// pid_configure(&PID, kp, ki, kd, ilimit); float Td; // Derivative time constant
float Ti = kp / ki; float Ti; // Integral time constant
float Td = kd / kp; float kt; // Feedback gain for integral windup avoidance
float Tt = (Ti + Td) / 2.0f; float N = 10.0f; // N is the factor used to determine the
float kt = 1.0f / Tt; // time constant for derivative filtering
float u0 = 0.0f; // Why 10? Maybe should be configurable?
float N = 10.0f; float Tf; // Low pass filter time constant for derivative filtering
float Tf = Td / N;
if (ki < 1e-6f) { float u0 = 0.0f;
// Avoid Ti being infinite
Ti = 1e6f; // Define Td, handling zero kp term (for I or ID controller)
// Tt antiwindup time constant - we don't need antiwindup with no I term if (kp < 1e-6f) {
Tt = 1e6f; Td = 1e6f;
kt = 0.0f; } else {
Td = kd / kp;
} }
// Define Ti, Tt and kt, handling zero ki term (for P or PD controller)
if (ki < 1e-6f) { // Avoid Ti being infinite
kt = 0.0f;
} else {
Ti = kp / ki;
kt = 1.0f / Ti;
}
// Define Tf, according to controller type
if (kd < 1e-6f) { if (kd < 1e-6f) {
// PI Controller // PI Controller or P Controller
Tf = Ti / N; Tf = 0; // Originally this: (Ti / N) - which creates a D term from
// the integral time constant!
} else {
Tf = Td / N;
} }
if (beta > 1.0f) { if (beta > 1.0f) {
@ -107,6 +120,8 @@ void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, fl
beta = 0.4f; beta = 0.4f;
} }
pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f); pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f); pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
deltaTime = dT; deltaTime = dT;