4 * This file is part of BeRTOS.
6 * Bertos 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 of the License, or
9 * (at your option) any later version.
11 * This program 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.
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 * As a special exception, you may use this file as part of a free software
21 * library without restriction. Specifically, if other files instantiate
22 * templates or use macros or inline functions from this file, or you compile
23 * this file and link it with other files to produce an executable, this
24 * file does not by itself cause the resulting executable to be covered by
25 * the GNU General Public License. This exception does not however
26 * invalidate any other reasons why the executable file might be covered by
27 * the GNU General Public License.
29 * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
33 * \brief Proportional, integral, derivative controller (PID controller) (implementation)
37 * \author Daniele Basile <asterix@develer.com>
40 #include "pid_control.h"
42 #include <cfg/debug.h>
45 * Compute next value for reaching \a target point.
47 piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
54 //Compute current error.
55 err = target - curr_pos;
58 * Compute Proportional contribute
60 P = err * pid_ctx->cfg->kp;
62 //Update integral state error
63 pid_ctx->i_state += err;
65 //Clamp integral state between i_min and i_max
66 pid_ctx->i_state = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
69 * Compute Integral contribute
71 * note: for computing the integral contribute we use a sample period in seconds
72 * and so we divide sample_period in microsenconds for 1000.
74 I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
78 * Compute derivative contribute
80 D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
83 // TRACEMSG("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
86 //Store the last error value
87 pid_ctx->prev_err = err;
88 piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
90 // TRACEMSG("pid[%lf]",pid);
92 //Clamp out between out_min and out_max
99 void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
102 * Init all values of pid control struct
104 pid_ctx->cfg = pid_cfg;
106 pid_control_reset(pid_ctx);