src/main/common/fp_pid.h

   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 #pragma once
  26
  27 #include <stdbool.h>
  28
  29 #include "common/filter.h"
  30 #include "common/maths.h"
  31
  32 typedef struct {
  33     float kP;
  34     float kI;
  35     float kD;
  36     float kT;   // Tracking gain (anti-windup)
  37     float kFF;  // FeedForward Component
  38 } pidControllerParam_t;
  39
  40 typedef enum {
  41     PID_DTERM_FROM_ERROR            = 1 << 0,
  42     PID_ZERO_INTEGRATOR             = 1 << 1,
  43     PID_SHRINK_INTEGRATOR           = 1 << 2,
  44     PID_LIMIT_INTEGRATOR            = 1 << 3,
  45     PID_FREEZE_INTEGRATOR           = 1 << 4,
  46 } pidControllerFlags_e;
  47
  48 typedef struct {
  49     bool reset;
  50     pidControllerParam_t param;
  51     pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
  52     float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
  53     float integrator;                   // integrator value
  54     float last_input;                   // last input for derivative
  55
  56     float integral;                     // used integral value in output
  57     float proportional;                 // used proportional value in output
  58     float derivative;                   // used derivative value in output
  59     float feedForward;                  // used FeedForward value in output
  60     float output_constrained;           // controller output constrained
  61 } pidController_t;
  62
  63 float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
  64 float navPidApply3(
  65     pidController_t *pid,
  66     const float setpoint,
  67     const float measurement,
  68     const float dt,
  69     const float outMin,
  70     const float outMax,
  71     const pidControllerFlags_e pidFlags,
  72     const float gainScaler,
  73     const float dTermScaler
  74 );
  75 void navPidReset(pidController_t *pid);
  76 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);