pid_control.c

Go to the documentation of this file.

#include "pid_control.h"

#include <cfg/debug.h>

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);


//  TRACEMSG("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);

//  TRACEMSG("pid[%lf]",pid);

    //Clamp out between out_min and out_max
    return pid;
}

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);

}