flight/libraries/math/pid.h

   1 /**
   2  ******************************************************************************
   3  * @file       pid.h
   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 #ifndef PID_H
  28 #define PID_H
  29
  30 #include "mathmisc.h"
  31
  32 // !
  33 struct pid {
  34     float p;
  35     float i;
  36     float d;
  37     float iLim;
  38     float iAccumulator;
  39     float lastErr;
  40     float lastDer;
  41 };
  42
  43 // pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
  44 struct pid2 {
  45     float   u0;   // Initial value of r & y - for bumpless transfer
  46     float   va;   // Actuator settings for scaling
  47     float   vb;
  48     float   kp;   // proportional gain
  49     float   bi;   // Integral gain . dT
  50     float   ad;   // Filtered factor for derivative calculation
  51     float   bd;   // Constant for derivative calculation
  52     float   br;   // Time constant for integral calculation
  53     float   beta; // Scalar for proportional factor
  54     float   yold; // t-1 y value for Integral calculation
  55     float   P;    // Latest calculated P, I & D values
  56     float   I;
  57     float   D;
  58     uint8_t reconfigure;
  59 };
  60
  61 typedef struct pid_scaler_s {
  62     float p;
  63     float i;
  64     float d;
  65 } pid_scaler;
  66
  67 // ! Methods to use the pid structures
  68 float pid_apply(struct pid *pid, const float err, float dT);
  69 float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term);
  70 void pid_zero(struct pid *pid);
  71 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
  72 void pid_configure_derivative(float cutoff, float gamma);
  73
  74 // Methods for use with pid2 structure
  75 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);
  76 void pid2_transfer(struct pid2 *pid, float u0);
  77 float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh);
  78
  79 #endif /* PID_H */