44 lines
757 B
C
44 lines
757 B
C
/*
|
|
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
|
*
|
|
*/
|
|
|
|
|
|
#include <stdint.h>
|
|
#include <pidcont.h>
|
|
|
|
void pidcont_init(pidcont_t* p) {
|
|
p->perror = 0.0;
|
|
p->integ = 0.0;
|
|
|
|
p->kp = 0.0;
|
|
p->ki = 0.0;
|
|
p->kd = 0.0;
|
|
}
|
|
|
|
|
|
void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
|
|
p->kp = kp;
|
|
p->ki = ki;
|
|
p->kd = kd;
|
|
}
|
|
|
|
double pidcont_apply(pidcont_t* p, double target, double actual, double dt) {
|
|
|
|
double error = 0.0;
|
|
double deriv = 0.0;
|
|
double integ = 0.0;
|
|
double output = 0.0;
|
|
|
|
error = target - actual;
|
|
integ += error * dt;
|
|
|
|
deriv = error - p->perror;
|
|
|
|
output = (p->kp * error) + (p->ki * integ) + (p->kd * deriv);
|
|
|
|
p->perror = error;
|
|
p->integ = integ;
|
|
return output;
|
|
}
|