pid.h

Go to the documentation of this file.

/*
 * Copyright (C) 2018 Gautier Hattenberger
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#ifndef PID_H
#define PID_H
 
struct PID_f {
  float u;        
  float e[2];     
  float sum;      
  float g[3];     
  float max_sum;  
};
 
static inline void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
{
  *pid = (struct PID_f) {
    0.f,
    { 0.f , 0.f },
    0.f,
    { Kp, Kd, Ki },
    max_sum
  };
}
 
static inline float update_pid_f(struct PID_f *pid, float value, float dt)
{
  pid->e[1] = pid->e[0];
  pid->e[0] = value;
  float integral = pid->g[2] * (pid->sum + value);
  if (integral > pid->max_sum) {
    integral = pid->max_sum;
  } else if (integral < -pid->max_sum) {
    integral = -pid->max_sum;
  } else {
    pid->sum += value;
  }
  pid->u = pid->g[0] * pid->e[0] + pid->g[1] * (pid->e[0] - pid->e[1]) / dt + integral;
  return pid->u;
}
 
static inline float get_pid_f(struct PID_f *pid)
{
  return pid->u;
}
 
static inline void reset_pid_f(struct PID_f *pid)
{
  pid->u = 0.f;
  pid->e[0] = 0.f;
  pid->e[1] = 0.f;
  pid->sum = 0.f;
}
 
static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
{
  pid->g[0] = Kp;
  pid->g[1] = Kd;
  pid->g[2] = Ki;
}
 
static inline void set_integral_pid_f(struct PID_f *pid, float value)
{
  float integral = value;
  if (integral < -pid->max_sum) {
    integral = -pid->max_sum;
  } else if (integral > pid->max_sum) {
    integral = pid->max_sum;
  }
  if (fabsf(pid->g[2]) < 1e-6) {
    pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum
  } else {
    pid->sum = integral / pid->g[2];
  }
}
 
 
 
struct PID_df {
  float u[2]; 
  float e[3]; 
  float g[3]; 
};
 
static inline void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
{
  *pid = (struct PID_df) {
    { 0.f, 0.f },
    { 0.f ,0.f , 0.f },
    { Kp + Ki * Ts / 2.f + Kd / Ts,
     -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts,
      Kd / Ts }
  };
}
 
static inline float update_pid_df(struct PID_df *pid, float value)
{
  pid->e[2] = pid->e[1];
  pid->e[1] = pid->e[0];
  pid->e[0] = value;
  pid->u[1] = pid->u[0];
  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * pid->e[2];
  return pid->u[0];
}
 
static inline float get_pid_df(struct PID_df *pid)
{
  return pid->u[0];
}
 
static inline void reset_pid_df(struct PID_df *pid)
{
  pid->u[0] = 0.f;
  pid->u[1] = 0.f;
  pid->e[0] = 0.f;
  pid->e[1] = 0.f;
  pid->e[2] = 0.f;
}
 
static inline void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
{
  pid->g[0] = Kp + Ki * Ts / 2.f + Kd / Ts;
  pid->g[1] = -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts;
  pid->g[2] = Kd / Ts;
}
 
struct PI_D_df {
  float u[2]; 
  float e[2]; 
  float g[3]; 
};
 
static inline void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
{
  *pid = (struct PI_D_df) {
    { 0.f, 0.f },
    { 0.f ,0.f },
    { Kp + Ki * Ts / 2.f,
     -Kp + Ki * Ts / 2.f,
      Kd }
  };
}
 
static inline float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
{
  pid->e[1] = pid->e[0];
  pid->e[0] = value;
  pid->u[1] = pid->u[0];
  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * deriv;
  return pid->u[0];
}
 
static inline float get_pi_d_df(struct PI_D_df *pid)
{
  return pid->u[0];
}
 
static inline void reset_pi_d_df(struct PI_D_df *pid)
{
  pid->u[0] = 0.f;
  pid->u[1] = 0.f;
  pid->e[0] = 0.f;
  pid->e[1] = 0.f;
}
 
static inline void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
{
  pid->g[0] = Kp + Ki * Ts / 2.f;
  pid->g[1] = -Kp + Ki * Ts / 2.f;
  pid->g[2] = Kd;
}
 
#endif