src/main/common/fp_pid.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include "platform.h"
  26
  27 #include <math.h>
  28 #include "common/fp_pid.h"
  29
  30 /*-----------------------------------------------------------
  31  * Float point PID-controller implementation
  32  *-----------------------------------------------------------*/
  33 // Implementation of PID with back-calculation I-term anti-windup
  34 // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
  35 // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
  36 float navPidApply3(
  37     pidController_t *pid,
  38     const float setpoint,
  39     const float measurement,
  40     const float dt,
  41     const float outMin,
  42     const float outMax,
  43     const pidControllerFlags_e pidFlags,
  44     const float gainScaler,
  45     const float dTermScaler
  46 ) {
  47     float newProportional, newDerivative, newFeedForward;
  48     float error = 0.0f;
  49
  50     if (pid->errorLpfHz > 0.0f) {
  51         error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt);
  52     } else {
  53         error = setpoint - measurement;
  54     }
  55
  56     /* P-term */
  57     newProportional = error * pid->param.kP * gainScaler;
  58
  59     /* D-term */
  60     if (pid->reset) {
  61         pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
  62         pid->reset = false;
  63     }
  64
  65     if (pidFlags & PID_DTERM_FROM_ERROR) {
  66         /* Error-tracking D-term */
  67         newDerivative = (error - pid->last_input) / dt;
  68         pid->last_input = error;
  69     } else {
  70         /* Measurement tracking D-term */
  71         newDerivative = -(measurement - pid->last_input) / dt;
  72         pid->last_input = measurement;
  73     }
  74
  75     if (pid->dTermLpfHz > 0.0f) {
  76         newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
  77     } else {
  78         newDerivative = pid->param.kD * newDerivative;
  79     }
  80
  81     newDerivative = newDerivative * gainScaler * dTermScaler;
  82
  83     if (pidFlags & PID_ZERO_INTEGRATOR) {
  84         pid->integrator = 0.0f;
  85     }
  86
  87     /*
  88      * Compute FeedForward parameter
  89      */
  90     newFeedForward = setpoint * pid->param.kFF * gainScaler;
  91
  92     /* Pre-calculate output and limit it if actuator is saturating */
  93     const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
  94     const float outValConstrained = constrainf(outVal, outMin, outMax);
  95     float backCalc = outValConstrained - outVal;//back-calculation anti-windup
  96     if (SIGN(backCalc) == SIGN(pid->integrator)) {
  97         //back calculation anti-windup can only shrink integrator, will not push it to the opposite direction
  98         backCalc = 0.0f;
  99     }
 100
 101     pid->proportional = newProportional;
 102     pid->integral = pid->integrator;
 103     pid->derivative = newDerivative;
 104     pid->feedForward = newFeedForward;
 105     pid->output_constrained = outValConstrained;
 106
 107     /* Update I-term */
 108     if (
 109         !(pidFlags & PID_ZERO_INTEGRATOR) &&
 110         !(pidFlags & PID_FREEZE_INTEGRATOR)
 111     ) {
 112         const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt);
 113
 114         if (pidFlags & PID_SHRINK_INTEGRATOR) {
 115             // Only allow integrator to shrink
 116             if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
 117                 pid->integrator = newIntegrator;
 118             }
 119         }
 120         else {
 121             pid->integrator = newIntegrator;
 122         }
 123     }
 124
 125     if (pidFlags & PID_LIMIT_INTEGRATOR) {
 126         pid->integrator = constrainf(pid->integrator, outMin, outMax);
 127     }
 128
 129     return outValConstrained;
 130 }
 131
 132 float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
 133 {
 134     return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
 135 }
 136
 137 void navPidReset(pidController_t *pid)
 138 {
 139     pid->reset = true;
 140     pid->proportional = 0.0f;
 141     pid->integral = 0.0f;
 142     pid->derivative = 0.0f;
 143     pid->integrator = 0.0f;
 144     pid->last_input = 0.0f;
 145     pid->feedForward = 0.0f;
 146     pt1FilterReset(&pid->dterm_filter_state, 0.0f);
 147     pid->output_constrained = 0.0f;
 148 }
 149
 150 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz)
 151 {
 152     pid->param.kP = _kP;
 153     pid->param.kI = _kI;
 154     pid->param.kD = _kD;
 155     pid->param.kFF = _kFF;
 156
 157     if (_kI > 1e-6f && _kP > 1e-6f) {
 158         float Ti = _kP / _kI;
 159         float Td = _kD / _kP;
 160         pid->param.kT = 2.0f / (Ti + Td);
 161     }
 162     else if (_kI > 1e-6f) {
 163         pid->param.kI = _kI;
 164         pid->param.kT = 0.0f;
 165     }
 166     else {
 167         pid->param.kI = 0.0;
 168         pid->param.kT = 0.0;
 169     }
 170     pid->dTermLpfHz = _dTermLpfHz;
 171     pid->errorLpfHz = _errorLpfHz;
 172     navPidReset(pid);
 173 }