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

LP-289 Manage kp=0 in PIDControlDown::UpdateParameters()

Browse Source
This commit is contained in:
paul Jewell 2016-12-10 09:09:53 +01:00
parent 556c2e9e99
commit 814a882a1d
1 changed files with 22 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
flight/libraries/pid/pidcontroldown.cpp
View File

@ -91,26 +91,39 @@ void PIDControlDown::Activate()
void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{
// pid_configure(&PID, kp, ki, kd, ilimit);
float Ti = kp / ki;
float Td = kd / kp;
float Tt = (Ti + Td) / 2.0f;
float kt = 1.0f / Tt;
float Td;
float Ti;
float Tt;
float kt;
float N = 10.0f;
float Tf = Td / N;
float Tf;
if (ki < 1e-6f) {
// Avoid Ti being infinite
// Define Td, handling zero kp term (for I or ID controller)
if (kp < 1e-6f) {
Td = 1e6f;
} 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
Ti = 1e6f;
// Tt antiwindup time constant - we don't need antiwindup with no I term
Tt = 1e6f;
kt = 0.0f;
} else {
Ti = kp / ki;
Tt = (Ti + Td) / 2.0f;
kt = 1.0f / Tt;
}
// Define Tf, according to controller type
if (kd < 1e-6f) {
// PI Controller or P Controller
Tf = Ti / N;
} else {
Tf = Td / N;
}
if (beta > 1.0f) {
beta = 1.0f;
} else if (beta < 0.4f) {