Changeset View
Changeset View
Standalone View
Standalone View
sys/kern/subr_pidctrl.c
Show All 29 Lines | |||||
__FBSDID("$FreeBSD$"); | __FBSDID("$FreeBSD$"); | ||||
#include <sys/param.h> | #include <sys/param.h> | ||||
#include <sys/kernel.h> | #include <sys/kernel.h> | ||||
#include <sys/systm.h> | #include <sys/systm.h> | ||||
#include <sys/sysctl.h> | #include <sys/sysctl.h> | ||||
#include <sys/pidctrl.h> | #include <sys/pidctrl.h> | ||||
int | |||||
pidctrl_error(struct pidctrl *pc, int input) | |||||
{ | |||||
return (pc->pc_setpoint - input); | |||||
} | |||||
void | void | ||||
pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound, | pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound, | ||||
int Kpd, int Kid, int Kdd) | int Kpd, int Kid, int Kdd) | ||||
{ | { | ||||
bzero(pc, sizeof(*pc)); | bzero(pc, sizeof(*pc)); | ||||
pc->pc_setpoint = setpoint; | pc->pc_setpoint = setpoint; | ||||
pc->pc_interval = interval; | pc->pc_interval = interval; | ||||
Show All 36 Lines | |||||
} | } | ||||
int | int | ||||
pidctrl_classic(struct pidctrl *pc, int input) | pidctrl_classic(struct pidctrl *pc, int input) | ||||
{ | { | ||||
int output, error; | int output, error; | ||||
int Kpd, Kid, Kdd; | int Kpd, Kid, Kdd; | ||||
error = pc->pc_setpoint - input; | error = pidctrl_error(pc, input); | ||||
pc->pc_ticks = ticks; | pc->pc_ticks = ticks; | ||||
pc->pc_olderror = pc->pc_error; | pc->pc_olderror = pc->pc_error; | ||||
/* Fetch gains and prevent divide by zero. */ | /* Fetch gains and prevent divide by zero. */ | ||||
Kpd = MAX(pc->pc_Kpd, 1); | Kpd = MAX(pc->pc_Kpd, 1); | ||||
Kid = MAX(pc->pc_Kid, 1); | Kid = MAX(pc->pc_Kid, 1); | ||||
Kdd = MAX(pc->pc_Kdd, 1); | Kdd = MAX(pc->pc_Kdd, 1); | ||||
Show All 14 Lines | |||||
} | } | ||||
int | int | ||||
pidctrl_daemon(struct pidctrl *pc, int input) | pidctrl_daemon(struct pidctrl *pc, int input) | ||||
{ | { | ||||
int output, error; | int output, error; | ||||
int Kpd, Kid, Kdd; | int Kpd, Kid, Kdd; | ||||
error = pc->pc_setpoint - input; | error = pidctrl_error(pc, input); | ||||
/* | /* | ||||
* When ticks expires we reset our variables and start a new | * When ticks expires we reset our variables and start a new | ||||
* interval. If we're called multiple times during one interval | * interval. If we're called multiple times during one interval | ||||
* we attempt to report a target as if the entire error came at | * we attempt to report a target as if the entire error came at | ||||
* the interval boundary. | * the interval boundary. | ||||
*/ | */ | ||||
if ((u_int)ticks - pc->pc_ticks >= pc->pc_interval) { | if ((u_int)ticks - pc->pc_ticks >= pc->pc_interval) { | ||||
pc->pc_ticks = ticks; | pc->pc_ticks = ticks; | ||||
Show All 28 Lines |