forked from haraldwolff/avr-fw-modules
151 lines
2.6 KiB
C
151 lines
2.6 KiB
C
#include <fixpoint/fixpoint.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
|
|
fp4816_pid_t* pid4816_create (void){
|
|
fp4816_pid_t *pid = malloc( sizeof( fp4816_pid_t) );
|
|
|
|
if (!pid)
|
|
return NULL;
|
|
|
|
memset(pid, 0x00, sizeof(fp4816_pid_t));
|
|
|
|
return pid;
|
|
};
|
|
|
|
int pid4816_set (fp4816_pid_t *pid,fp4816_t Kp,fp4816_t Ki,fp4816_t Kd){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
pid->kp = Kp;
|
|
pid->ki = Ki;
|
|
pid->kd = Kd;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int pid4816_set_kp (fp4816_pid_t *pid,fp4816_t Kp){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
pid->kp = Kp;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int pid4816_set_ki (fp4816_pid_t *pid,fp4816_t Ki){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
pid->ki = Ki;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
int pid4816_set_kd (fp4816_pid_t *pid,fp4816_t Kd){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
pid->kd = Kd;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int pid4816_get_kp (fp4816_pid_t *pid,fp4816_t *Kp){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
*Kp = pid->kp;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
int pid4816_get_ki (fp4816_pid_t *pid,fp4816_t *Ki){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
*Ki = pid->ki;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
int pid4816_get_kd (fp4816_pid_t *pid,fp4816_t *Kd){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
*Kd = pid->kd;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int pid4816_set_limits(fp4816_pid_t *pid,fp4816_t *omin,fp4816_t *omax,fp4816_t *imin,fp4816_t *imax){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
if (omin)
|
|
pid->lim_omin = *omin;
|
|
if (omax)
|
|
pid->lim_omax = *omax;
|
|
if (imin)
|
|
pid->lim_imin = *imin;
|
|
if (imax)
|
|
pid->lim_imax = *imax;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int pid4816_get_limits(fp4816_pid_t *pid,fp4816_t *omin,fp4816_t *omax,fp4816_t *imin,fp4816_t *imax){
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
if (omin)
|
|
*omin = pid->lim_omin;
|
|
if (omax)
|
|
*omax = pid->lim_omax;
|
|
if (imin)
|
|
*imin = pid->lim_imin;
|
|
if (imax)
|
|
*imax = pid->lim_imax;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
|
|
int pid4816_cycle (fp4816_pid_t *pid,fp4816_t value){
|
|
fp4816_t t;
|
|
|
|
if (!pid)
|
|
return -ENULLPTR;
|
|
|
|
pid->integral += fp4816_mul(pid->ki, value);
|
|
if (pid->integral < pid->lim_imin)
|
|
pid->integral = pid->lim_imin;
|
|
if (pid->integral > pid->lim_imax)
|
|
pid->integral = pid->lim_imax;
|
|
|
|
t = pid->integral + fp4816_mul( pid->kp, value ) + fp4816_mul( pid->kd, ( value - pid->last_value ) );
|
|
pid->last_value = value;
|
|
|
|
if (t < pid->lim_omin)
|
|
t = pid->lim_omin;
|
|
if (t > pid->lim_omax)
|
|
t = pid->lim_omax;
|
|
|
|
pid->out = t;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
/*
|
|
static inline fp4816_t pi_fp4816_cycle(PI_FP4816 *pi,fp4816_t value)
|
|
{
|
|
pi->integral += fp4816_mul(pi->ki, value);
|
|
if (pi->integral > pi->limit_max)
|
|
pi->integral = pi->limit_max;
|
|
if (pi->integral < pi->limit_min)
|
|
pi->integral = pi->limit_min;
|
|
return pi->integral + fp4816_mul(pi->kp, value);
|
|
};
|
|
*
|
|
*/ |