2 * Copyright Droids Corporation, Microb Technology, Eirbot (2006)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: pid.c,v 1.5.4.10 2009-03-05 22:02:55 zer0 Exp $
26 /** this function will initialize all fieds of pid structure to 0 */
27 void pid_init(struct pid_filter *p)
31 memset(p, 0, sizeof(*p));
33 p->derivate_nb_samples = 1;
37 /** this function will initialize all fieds of pid structure to 0,
38 * except configuration */
39 void pid_reset(struct pid_filter *p)
43 memset(p->prev_samples, 0, sizeof(p->prev_samples));
50 void pid_set_gains(struct pid_filter *p, int16_t gp, int16_t gi, int16_t gd)
60 void pid_set_maximums(struct pid_filter *p, int32_t max_in, int32_t max_I, int32_t max_out)
70 void pid_set_out_shift(struct pid_filter *p, uint8_t out_shift)
74 p->out_shift=out_shift;
78 int8_t pid_set_derivate_filter(struct pid_filter *p, uint8_t nb_samples)
83 if (nb_samples > PID_DERIVATE_FILTER_MAX_SIZE) {
87 p->derivate_nb_samples = nb_samples;
94 int16_t pid_get_gain_P(struct pid_filter *p)
99 int16_t pid_get_gain_I(struct pid_filter *p)
104 int16_t pid_get_gain_D(struct pid_filter *p)
110 int32_t pid_get_max_in(struct pid_filter *p)
115 int32_t pid_get_max_I(struct pid_filter *p)
120 int32_t pid_get_max_out(struct pid_filter *p)
126 uint8_t pid_get_out_shift(struct pid_filter *p)
128 return (p->out_shift);
131 uint8_t pid_get_derivate_filter(struct pid_filter *p)
133 return (p->derivate_nb_samples);
136 int32_t pid_get_value_I(struct pid_filter *p)
146 int32_t pid_get_value_in(struct pid_filter *p)
151 ret = p->prev_samples[p->index];
156 int32_t pid_get_value_D(struct pid_filter *p)
166 int32_t pid_get_value_out(struct pid_filter *p)
176 /* first parameter should be a (struct pid_filter *) */
177 int32_t pid_do_filter(void * data, int32_t in)
181 struct pid_filter * p = data;
185 * Integral value : the integral become bigger with time .. (think
186 * to area of graph, we add one area to the previous) so,
187 * integral = previous integral + current value
191 * f(t+h) - f(t) with f(t+h) = current value
192 * derivate = ------------- f(t) = previous value
194 * so derivate = current error - previous error
196 * We can apply a filter to reduce noise on the derivate term,
197 * by using a bigger period.
200 prev_index = p->index + 1;
201 if (prev_index >= p->derivate_nb_samples)
204 /* saturate input... it influences integral an derivate */
206 S_MAX(in, p->max_in) ;
208 derivate = in - p->prev_samples[prev_index];
212 S_MAX(p->integral, p->max_I) ;
214 /* so, command = P.coef_P + I.coef_I + D.coef_D */
215 command = in * p->gain_P +
216 p->integral * p->gain_I +
217 (derivate * p->gain_D) / p->derivate_nb_samples ;
220 command = -( -command >> p->out_shift );
222 command = command >> p->out_shift ;
225 S_MAX (command, p->max_out) ;
228 /* backup of current error value (for the next calcul of derivate value) */
229 p->prev_samples[p->index] = in ;
230 p->index = prev_index; /* next index is prev_index */
231 p->prev_D = derivate ;
232 p->prev_out = command ;