flight/libraries/math/pid.c

   1 /**
   2  ******************************************************************************
   3  * @file       pid.c
   4  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
   5  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   6  * @brief      Methods to work with PID structure
   7  *
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 #include "openpilot.h"
  28 #include "pid.h"
  29 #include <mathmisc.h>
  30 #include <pios_math.h>
  31
  32 // ! Store the shared time constant for the derivative cutoff.
  33 static float deriv_tau   = 7.9577e-3f;
  34
  35 // ! Store the setpoint weight to apply for the derivative term
  36 static float deriv_gamma = 1.0f;
  37
  38 /**
  39  * Update the PID computation
  40  * @param[in] pid The PID struture which stores temporary information
  41  * @param[in] err The error term
  42  * @param[in] dT  The time step
  43  * @returns Output the computed controller value
  44  */
  45 float pid_apply(struct pid *pid, const float err, float dT)
  46 {
  47     // Scale up accumulator by 1000 while computing to avoid losing precision
  48     pid->iAccumulator += err * (pid->i * dT * 1000.0f);
  49     pid->iAccumulator  = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
  50
  51     // Calculate DT1 term
  52     float diff  = (err - pid->lastErr);
  53     float dterm = 0;
  54     pid->lastErr = err;
  55     if (pid->d > 0.0f && dT > 0.0f) {
  56         dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
  57         pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
  58     } // 7.9577e-3  means 20 Hz f_cutoff
  59
  60     return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm;
  61 }
  62
  63 /**
  64  * Update the PID computation with setpoint weighting on the derivative
  65  * @param[in] pid The PID struture which stores temporary information
  66  * @param[in] factor A dynamic factor to scale pid's by, to compensate nonlinearities
  67  * @param[in] setpoint The setpoint to use
  68  * @param[in] measured The measured value of output
  69  * @param[in] dT  The time step
  70  * @returns Output the computed controller value
  71  *
  72  * This version of apply uses setpoint weighting for the derivative component so the gain
  73  * on the gyro derivative can be different than the gain on the setpoint derivative
  74  */
  75 float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term)
  76 {
  77     float err = setpoint - measured;
  78
  79     // Scale up accumulator by 1000 while computing to avoid losing precision
  80     pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f);
  81     pid->iAccumulator  = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
  82
  83     // Calculate DT1 term,
  84     float diff;
  85     float derr = (-measured);
  86
  87     if (!meas_based_d_term) {
  88         derr += deriv_gamma * setpoint;
  89     }
  90
  91     diff = derr - pid->lastErr;
  92     pid->lastErr = derr;
  93
  94     float dterm = 0;
  95
  96     if (pid->d > 0.0f && dT > 0.0f) {
  97         // low pass filter derivative term. below formula is the same as
  98         // dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT
  99         // with alpha = dT/(deriv_tau+dT)
 100         dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
 101         pid->lastDer = dterm;
 102     }
 103     return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
 104 }
 105
 106 /**
 107  * Reset a bit
 108  * @param[in] pid The pid to reset
 109  */
 110 void pid_zero(struct pid *pid)
 111 {
 112     if (!pid) {
 113         return;
 114     }
 115
 116     pid->iAccumulator = 0;
 117     pid->lastErr = 0;
 118     pid->lastDer = 0;
 119 }
 120
 121 /**
 122  * @brief Configure the common terms that alter ther derivative
 123  * @param[in] cutoff The cutoff frequency (in Hz)
 124  * @param[in] gamma The gamma term for setpoint shaping (unsused now)
 125  */
 126 void pid_configure_derivative(float cutoff, float g)
 127 {
 128     deriv_tau   = 1.0f / (2 * M_PI_F * cutoff);
 129     deriv_gamma = g;
 130 }
 131
 132 /**
 133  * Configure the settings for a pid structure
 134  * @param[out] pid The PID structure to configure
 135  * @param[in] p The proportional term
 136  * @param[in] i The integral term
 137  * @param[in] d The derivative term
 138  */
 139 void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
 140 {
 141     if (!pid) {
 142         return;
 143     }
 144
 145     pid->p    = p;
 146     pid->i    = i;
 147     pid->d    = d;
 148     pid->iLim = iLim;
 149 }
 150
 151
 152 /**
 153  * Configure the settings for a pid2 structure
 154  * @param[out] pid The PID2 structure to configure
 155  * @param[in] kp proportional gain
 156  * @param[in] ki integral gain.  Time constant Ti = kp/ki
 157  * @param[in] kd derivative gain. Time constant Td = kd/kp
 158  * @param[in] Tf filtering time = (kd/kp)/N, N is in the range of 2 to 20
 159  * @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
 160  * @param[in] dt delta time increment
 161  * @param[in] beta setpoint weight on setpoint in P component.  beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
 162  * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
 163  * @param[in] va constant for compute of actuator output for check against limits for antiwindup
 164  * @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup
 165  */
 166 void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb)
 167 {
 168     pid->reconfigure = true;
 169     pid->u0   = u0;
 170     pid->va   = va;
 171     pid->vb   = vb;
 172     pid->kp   = kp;
 173     pid->beta = beta; // setpoint weight on proportional term
 174
 175     pid->bi   = ki * dT;
 176     pid->br   = kt * dT / vb;
 177
 178     pid->ad   = Tf / (Tf + dT);
 179     pid->bd   = kd / (Tf + dT);
 180 }
 181
 182 /**
 183  * Achieve a bumpless transfer and trigger initialisation of I term
 184  * @param[out] pid The PID structure to configure
 185  * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
 186  */
 187 void pid2_transfer(struct pid2 *pid, float u0)
 188 {
 189     pid->reconfigure = true;
 190     pid->u0 = u0;
 191 }
 192
 193 /**
 194  * pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable
 195  * See "Feedback Systems" for an explanation
 196  * @param[out] pid The PID structure to configure
 197  * @param[in] r setpoint
 198  * @param[in] y process variable
 199  * @param[in] ulow lower limit on actuator
 200  * @param[in] uhigh upper limit on actuator
 201  */
 202 float pid2_apply(
 203     struct pid2 *pid,
 204     const float r,
 205     const float y,
 206     const float ulow,
 207     const float uhigh)
 208 {
 209     // on reconfigure ensure bumpless transfer
 210     // http://www.controlguru.com/2008/021008.html
 211     if (pid->reconfigure) {
 212         pid->reconfigure = false;
 213
 214         // initialise derivative terms
 215         pid->yold = y;
 216         pid->D    = 0.0f;
 217
 218         // t=0, u=u0, y=y0, v=u
 219         // pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
 220         // this is dangerous, if pid->I starts nonzero with very low or zero kI, then
 221         // it will never settle!
 222         // pid->I = 0.0f;
 223         // whether ki is zero or non-zero, when doing 'reconfigure' we start with setpoint==actual
 224         pid->I = (pid->u0 - pid->va) / pid->vb;
 225         // if ki is non-zero
 226         if (pid->bi > 5e-7f) {
 227             // then the output can wind up/down to get rid of a difference between setpoint and actual
 228             // so we can generate the full bumpless transfer here
 229             // else leave off the part that can vary due to difference between setpoint and actual
 230             // so that it is repeatable (for tuning purposes at least)
 231             pid->I -= pid->kp * (pid->beta * r - y);
 232         }
 233     }
 234
 235     // compute proportional part
 236     pid->P = pid->kp * (pid->beta * r - y);
 237
 238     // update derivative part
 239     pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold);
 240
 241     // compute temporary output
 242     float v = pid->va + pid->vb * (pid->P + pid->I + pid->D);
 243
 244     // simulate actuator saturation
 245     float u = boundf(v, ulow, uhigh);
 246
 247     // update integral
 248     pid->I    = pid->I + pid->bi * (r - y) + pid->br * (u - v);
 249
 250     // update old process output
 251     pid->yold = y;
 252
 253     return u;
 254 }