Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pid.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
26 #ifndef PID_H
27 #define PID_H
28 
42 struct PID_f {
43  float u;
44  float e[2];
45  float sum;
46  float g[3];
47  float max_sum;
48 };
49 
50 static inline void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
51 {
52  *pid = (struct PID_f) {
53  0.f,
54  { 0.f , 0.f },
55  0.f,
56  { Kp, Kd, Ki },
57  max_sum
58  };
59 }
60 
68 static inline float update_pid_f(struct PID_f *pid, float value, float dt)
69 {
70  pid->e[1] = pid->e[0];
71  pid->e[0] = value;
72  float integral = pid->g[2] * (pid->sum + value);
73  if (integral > pid->max_sum) {
74  integral = pid->max_sum;
75  } else if (integral < -pid->max_sum) {
76  integral = -pid->max_sum;
77  } else {
78  pid->sum += value;
79  }
80  pid->u = pid->g[0] * pid->e[0] + pid->g[1] * (pid->e[0] - pid->e[1]) / dt + integral;
81  return pid->u;
82 }
83 
89 static inline float get_pid_f(struct PID_f *pid)
90 {
91  return pid->u;
92 }
93 
98 static inline void reset_pid_f(struct PID_f *pid)
99 {
100  pid->u = 0.f;
101  pid->e[0] = 0.f;
102  pid->e[1] = 0.f;
103  pid->sum = 0.f;
104 }
105 
113 static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
114 {
115  pid->g[0] = Kp;
116  pid->g[1] = Kd;
117  pid->g[2] = Ki;
118 }
119 
126 static inline void set_integral_pid_f(struct PID_f *pid, float value)
127 {
128  float integral = value;
129  if (integral < -pid->max_sum) {
130  integral = -pid->max_sum;
131  } else if (integral > pid->max_sum) {
132  integral = pid->max_sum;
133  }
134  if (fabsf(pid->g[2]) < 1e-6) {
135  pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum
136  } else {
137  pid->sum = integral / pid->g[2];
138  }
139 }
140 
141 
142 
160 struct PID_df {
161  float u[2];
162  float e[3];
163  float g[3];
164 };
165 
174 static inline void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
175 {
176  *pid = (struct PID_df) {
177  { 0.f, 0.f },
178  { 0.f ,0.f , 0.f },
179  { Kp + Ki * Ts / 2.f + Kd / Ts,
180  -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts,
181  Kd / Ts }
182  };
183 }
184 
191 static inline float update_pid_df(struct PID_df *pid, float value)
192 {
193  pid->e[2] = pid->e[1];
194  pid->e[1] = pid->e[0];
195  pid->e[0] = value;
196  pid->u[1] = pid->u[0];
197  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * pid->e[2];
198  return pid->u[0];
199 }
200 
206 static inline float get_pid_df(struct PID_df *pid)
207 {
208  return pid->u[0];
209 }
210 
215 static inline void reset_pid_df(struct PID_df *pid)
216 {
217  pid->u[0] = 0.f;
218  pid->u[1] = 0.f;
219  pid->e[0] = 0.f;
220  pid->e[1] = 0.f;
221  pid->e[2] = 0.f;
222 }
223 
232 static inline void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
233 {
234  pid->g[0] = Kp + Ki * Ts / 2.f + Kd / Ts;
235  pid->g[1] = -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts;
236  pid->g[2] = Kd / Ts;
237 }
238 
259 struct PI_D_df {
260  float u[2];
261  float e[2];
262  float g[3];
263 };
264 
273 static inline void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
274 {
275  *pid = (struct PI_D_df) {
276  { 0.f, 0.f },
277  { 0.f ,0.f },
278  { Kp + Ki * Ts / 2.f,
279  -Kp + Ki * Ts / 2.f,
280  Kd }
281  };
282 }
283 
291 static inline float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
292 {
293  pid->e[1] = pid->e[0];
294  pid->e[0] = value;
295  pid->u[1] = pid->u[0];
296  pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * deriv;
297  return pid->u[0];
298 }
299 
305 static inline float get_pi_d_df(struct PI_D_df *pid)
306 {
307  return pid->u[0];
308 }
309 
314 static inline void reset_pi_d_df(struct PI_D_df *pid)
315 {
316  pid->u[0] = 0.f;
317  pid->u[1] = 0.f;
318  pid->e[0] = 0.f;
319  pid->e[1] = 0.f;
320 }
321 
330 static inline void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
331 {
332  pid->g[0] = Kp + Ki * Ts / 2.f;
333  pid->g[1] = -Kp + Ki * Ts / 2.f;
334  pid->g[2] = Kd;
335 }
336 
337 #endif
338 
PID_f::sum
float sum
integral of input
Definition: pid.h:45
update_pid_f
static float update_pid_f(struct PID_f *pid, float value, float dt)
Update PID with a new value and return new command.
Definition: pid.h:68
PID_f::g
float g[3]
controller gains (Kp, Kd, Ki)
Definition: pid.h:46
update_pi_d_df
static float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
Update PI-D with a new value and return new command.
Definition: pid.h:291
init_pid_df
static void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
Init PID struct.
Definition: pid.h:174
set_gains_pi_d_df
static void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
Set gains PI-D struct.
Definition: pid.h:330
PID_df::e
float e[3]
input
Definition: pid.h:162
reset_pid_f
static void reset_pid_f(struct PID_f *pid)
Reset PID struture, gains left unchanged.
Definition: pid.h:98
PI_D_df::u
float u[2]
output
Definition: pid.h:260
PI_D_df
Distcrete time PI-D structure.
Definition: pid.h:259
PID_df::u
float u[2]
output
Definition: pid.h:161
PID_f::e
float e[2]
input
Definition: pid.h:44
set_gains_pid_df
static void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
Set gains of the PID struct.
Definition: pid.h:232
PI_D_df::g
float g[3]
controller gains
Definition: pid.h:262
PID_df::g
float g[3]
controller gains
Definition: pid.h:163
get_pi_d_df
static float get_pi_d_df(struct PI_D_df *pid)
Get current value of the PI-D command.
Definition: pid.h:305
set_gains_pid_f
static void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
Set gains of the PID struct.
Definition: pid.h:113
PID_f
Simple PID structure floating point.
Definition: pid.h:42
reset_pid_df
static void reset_pid_df(struct PID_df *pid)
Reset PID struture, gains left unchanged.
Definition: pid.h:215
init_pid_f
static void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
Definition: pid.h:50
get_pid_f
static float get_pid_f(struct PID_f *pid)
Get current value of the PID command.
Definition: pid.h:89
PI_D_df::e
float e[2]
input
Definition: pid.h:261
PID_f::u
float u
output
Definition: pid.h:43
PID_f::max_sum
float max_sum
windup protection, max of Ki * sum(e_k * dt)
Definition: pid.h:47
PID_df
Distcrete time PID structure.
Definition: pid.h:160
reset_pi_d_df
static void reset_pi_d_df(struct PI_D_df *pid)
Reset PI-D struture, gains left unchanged.
Definition: pid.h:314
get_pid_df
static float get_pid_df(struct PID_df *pid)
Get current value of the PID command.
Definition: pid.h:206
set_integral_pid_f
static void set_integral_pid_f(struct PID_f *pid, float value)
Set integral part, can be used to reset.
Definition: pid.h:126
update_pid_df
static float update_pid_df(struct PID_df *pid, float value)
Update PID with a new value and return new command.
Definition: pid.h:191
init_pi_d_df
static void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
Init PI-D struct.
Definition: pid.h:273