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

Merged in paul_jewell/librepilot/LP-289_pidcontrol_ne (pull request #362)

Browse Source 
LP-289 pidcontrol ne

Approved-by: Eric Price <corvuscorax@cybertrench.com>
Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Paul Jewell <teulupaul@gmail.com>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
This commit is contained in:
Paul Jewell 2017-03-21 21:49:51 +00:00 committed by Lalanne Laurent
commit 7428ab59f9
4 changed files with 74 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/libraries/math/pid.c
View File

@ -1,12 +1,8 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @see The GNU Public License (GPL) Version 3
@ -159,7 +155,7 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
* @param[in] kp proportional gain
* @param[in] ki integral gain. Time constant Ti = kp/ki
* @param[in] kd derivative gain. Time constant Td = kd/kp
* @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20
* @param[in] Tf filtering time = (kd/kp)/N, N is in the range of 2 to 20
* @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
* @param[in] dt delta time increment
* @param[in] beta setpoint weight on setpoint in P component. beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
@ -220,7 +216,10 @@ float pid2_apply(
pid->D = 0.0f;
// t=0, u=u0, y=y0, v=u
pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
// pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
// this is dangerous, if pid->I starts nonzero with very low or zero kI, then
// it will never settle!
pid->I = 0.0f;
}
// compute proportional part

28
flight/libraries/math/pid.h
View File

@ -1,12 +1,8 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @see The GNU Public License (GPL) Version 3
@ -46,17 +42,17 @@ struct pid {
// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
struct pid2 {
float u0;
float va;
float u0; // Initial value of r & y - for bumpless transfer
float va; // Actuator settings for scaling
float vb;
float kp;
float bi;
float ad;
float bd;
float br;
float beta;
float yold;
float P;
float kp; // proportional gain
float bi; // Integral gain . dT
float ad; // Filtered factor for derivative calculation
float bd; // Constant for derivative calculation
float br; // Time constant for integral calculation
float beta; // Scalar for proportional factor
float yold; // t-1 y value for Integral calculation
float P; // Latest calculated P, I & D values
float I;
float D;
uint8_t reconfigure;

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

@ -7,7 +7,8 @@
* @{
*
* @file pidcontroldown.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control for down direction
*
* @see The GNU Public License (GPL) Version 3
@ -90,25 +91,35 @@ 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 N = 10.0f;
float Tf = Td / N;
float Td; // Derivative time constant
float Ti; // Integral time constant
float kt; // Feedback gain for integral windup avoidance
float N = 10.0f; // N is the factor used to determine the
// time constant for derivative filtering
// Why 10? Maybe should be configurable?
float Tf; // Low pass filter time constant for derivative filtering
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;
// 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
kt = 0.0f;
} else {
Ti = kp / ki;
kt = 1.0f / Ti;
}
// Define Tf, according to controller type
if (kd < 1e-6f) {
// PI Controller or P Controller
Tf = Ti / N;
Tf = 0;
} else {
Tf = Td / N;
}
if (beta > 1.0f) {

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

@ -7,7 +7,8 @@
* @{
*
* @file PIDControlNE.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control loops for NE directions
*
* @see The GNU Public License (GPL) Version 3
@ -79,26 +80,37 @@ void PIDControlNE::Activate()
void PIDControlNE::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 u0 = 0.0f;
float N = 10.0f;
float Tf = Td / N;
float Td; // Derivative time constant
float Ti; // Integral time constant
float kt; // Feedback gain for integral windup avoidance
float N = 10.0f; // N is the factor used to determine the
// time constant for derivative filtering
// Why 10? Maybe should be configurable?
float Tf; // Low pass filter time constant for derivative filtering
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;
float u0 = 0.0f;
// 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
kt = 0.0f;
} else {
Ti = kp / ki;
kt = 1.0f / Ti;
}
// Define Tf, according to controller type
if (kd < 1e-6f) {
// PI Controller
Tf = Ti / N;
// PI Controller or P Controller
Tf = 0;
} else {
Tf = Td / N;
}
if (beta > 1.0f) {