PX4 Firmware
PX4 Autopilot Software http://px4.io
pid.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
4  * Author: Laurens Mackay <mackayl@student.ethz.ch>
5  * Tobias Naegeli <naegelit@student.ethz.ch>
6  * Martin Rutschmann <rutmarti@student.ethz.ch>
7  * Anton Babushkin <anton.babushkin@me.com>
8  * Julian Oes <joes@student.ethz.ch>
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * 1. Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * 2. Redistributions in binary form must reproduce the above copyright
17  * notice, this list of conditions and the following disclaimer in
18  * the documentation and/or other materials provided with the
19  * distribution.
20  * 3. Neither the name PX4 nor the names of its contributors may be
21  * used to endorse or promote products derived from this software
22  * without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
31  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
32  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  ****************************************************************************/
38 
39 /**
40  * @file pid.c
41  *
42  * Implementation of generic PID controller.
43  *
44  * @author Laurens Mackay <mackayl@student.ethz.ch>
45  * @author Tobias Naegeli <naegelit@student.ethz.ch>
46  * @author Martin Rutschmann <rutmarti@student.ethz.ch>
47  * @author Anton Babushkin <anton.babushkin@me.com>
48  * @author Julian Oes <joes@student.ethz.ch>
49  */
50 
51 #include "pid.h"
52 #include <math.h>
53 #include <px4_platform_common/defines.h>
54 
55 #define SIGMA 0.000001f
56 
57 __EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
58 {
59  pid->mode = mode;
60  pid->dt_min = dt_min;
61  pid->kp = 0.0f;
62  pid->ki = 0.0f;
63  pid->kd = 0.0f;
64  pid->integral = 0.0f;
65  pid->integral_limit = 0.0f;
66  pid->output_limit = 0.0f;
67  pid->error_previous = 0.0f;
68  pid->last_output = 0.0f;
69 }
70 
71 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
72 {
73  int ret = 0;
74 
75  if (PX4_ISFINITE(kp)) {
76  pid->kp = kp;
77 
78  } else {
79  ret = 1;
80  }
81 
82  if (PX4_ISFINITE(ki)) {
83  pid->ki = ki;
84 
85  } else {
86  ret = 1;
87  }
88 
89  if (PX4_ISFINITE(kd)) {
90  pid->kd = kd;
91 
92  } else {
93  ret = 1;
94  }
95 
96  if (PX4_ISFINITE(integral_limit)) {
97  pid->integral_limit = integral_limit;
98 
99  } else {
100  ret = 1;
101  }
102 
103  if (PX4_ISFINITE(output_limit)) {
104  pid->output_limit = output_limit;
105 
106  } else {
107  ret = 1;
108  }
109 
110  return ret;
111 }
112 
113 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
114 {
115  if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
116  return pid->last_output;
117  }
118 
119  float i, d;
120 
121  /* current error value */
122  float error = sp - val;
123 
124  /* current error derivative */
125  if (pid->mode == PID_MODE_DERIVATIV_CALC) {
126  d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
127  pid->error_previous = error;
128 
129  } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
130  d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
131  pid->error_previous = -val;
132 
133  } else if (pid->mode == PID_MODE_DERIVATIV_SET) {
134  d = -val_dot;
135 
136  } else {
137  d = 0.0f;
138  }
139 
140  if (!PX4_ISFINITE(d)) {
141  d = 0.0f;
142  }
143 
144  /* calculate PD output */
145  float output = (error * pid->kp) + (d * pid->kd);
146 
147  if (pid->ki > SIGMA) {
148  // Calculate the error integral and check for saturation
149  i = pid->integral + (error * dt);
150 
151  /* check for saturation */
152  if (PX4_ISFINITE(i)) {
153  if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
154  fabsf(i) <= pid->integral_limit) {
155  /* not saturated, use new integral value */
156  pid->integral = i;
157  }
158  }
159 
160  /* add I component to output */
161  output += pid->integral * pid->ki;
162  }
163 
164  /* limit output */
165  if (PX4_ISFINITE(output)) {
166  if (pid->output_limit > SIGMA) {
167  if (output > pid->output_limit) {
168  output = pid->output_limit;
169 
170  } else if (output < -pid->output_limit) {
171  output = -pid->output_limit;
172  }
173  }
174 
175  pid->last_output = output;
176  }
177 
178  return pid->last_output;
179 }
180 
181 
183 {
184  pid->integral = 0.0f;
185 }
float last_output
Definition: pid.h:81
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
Definition: pid.cpp:57
float integral_limit
Definition: pid.h:78
float error_previous
Definition: pid.h:80
Definition: I2C.hpp:51
float kp
Definition: pid.h:74
pid_mode_t mode
Definition: pid.h:72
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
Definition: pid.cpp:113
__BEGIN_DECLS enum PID_MODE pid_mode_t
float ki
Definition: pid.h:75
float dt_min
Definition: pid.h:73
float integral
Definition: pid.h:77
#define SIGMA
Definition: pid.cpp:55
Definition: pid.h:71
float kd
Definition: pid.h:76
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
Definition: pid.cpp:71
float dt
Definition: px4io.c:73
float output_limit
Definition: pid.h:79
__EXPORT void pid_reset_integral(PID_t *pid)
Definition: pid.cpp:182
mode
Definition: vtol_type.h:76
Definition of generic PID controller.