Git-Mirrors/OpenModem
1
0
Fork 0
mirror of https://github.com/markqvist/OpenModem.git synced 2025-05-04 07:25:09 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Working

Browse source
This commit is contained in:
Mark Qvist 2014-04-03 22:21:37 +02:00
commit c898b090dd
1049 changed files with 288572 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

115
bertos/algo/pid_control.c Normal file
View file

@ -0,0 +1,115 @@
/**
* \file
* <!--
* This file is part of BeRTOS.
*
* Bertos 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 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* As a special exception, you may use this file as part of a free software
* library without restriction. Specifically, if other files instantiate
* templates or use macros or inline functions from this file, or you compile
* this file and link it with other files to produce an executable, this
* file does not by itself cause the resulting executable to be covered by
* the GNU General Public License. This exception does not however
* invalidate any other reasons why the executable file might be covered by
* the GNU General Public License.
*
* Copyright 2008 Develer S.r.l. (http://www.develer.com/)
* -->
*
*
* \brief Proportional, integral, derivative controller (PID controller) (implementation)
*
*
* \author Daniele Basile <asterix@develer.com>
*/
#include "pid_control.h"
#include "cfg/cfg_pid.h"
// Define logging setting (for cfg/log.h module).
#define LOG_LEVEL PID_LOG_LEVEL
#define LOG_VERBOSITY PID_LOG_FORMAT
#include <cfg/log.h>
#include <cfg/debug.h>
/**
* Compute next value for reaching \a target point.
*/
piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
{
piddata_t P;
piddata_t I;
piddata_t D;
piddata_t err;
//Compute current error.
err = target - curr_pos;
/*
* Compute Proportional contribute
*/
P = err * pid_ctx->cfg->kp;
//Update integral state error
pid_ctx->i_state += err;
//Clamp integral state between i_min and i_max
pid_ctx->i_state = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
/*
* Compute Integral contribute
*
* note: for computing the integral contribute we use a sample period in seconds
* and so we divide sample_period in microsenconds for 1000.
*/
I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
/*
* Compute derivative contribute
*/
D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
//Store the last error value
pid_ctx->prev_err = err;
piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
LOG_INFO("pid[%lf]",pid);
//Clamp out between out_min and out_max
return pid;
}
/**
* Init PID control.
*/
void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
{
/*
* Init all values of pid control struct
*/
pid_ctx->cfg = pid_cfg;
pid_control_reset(pid_ctx);
}