Changeset View
Changeset View
Standalone View
Standalone View
sys/kern/subr_pidctrl.c
Context not available. | |||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "error", CTLFLAG_RD, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "error", CTLFLAG_RD, | ||||
&pc->pc_error, 0, "Current difference from setpoint value (P)"); | &pc->pc_error, 0, "Current difference from setpoint value (P)"); | ||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "mean error", CTLFLAG_RD, | |||||
&pc->pc_err_mean, 0, "Average difference from setpoint value (P)"); | |||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "error variance", CTLFLAG_RD, | |||||
&pc->pc_err_var, 0, "Squared average difference from average setpoint value (P)"); | |||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "olderror", CTLFLAG_RD, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "olderror", CTLFLAG_RD, | ||||
&pc->pc_olderror, 0, "Error value from last interval"); | &pc->pc_olderror, 0, "Error value from last interval"); | ||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "integral", CTLFLAG_RD, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "integral", CTLFLAG_RD, | ||||
Context not available. | |||||
&pc->pc_input, 0, "Last controller process variable input"); | &pc->pc_input, 0, "Last controller process variable input"); | ||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "output", CTLFLAG_RD, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "output", CTLFLAG_RD, | ||||
&pc->pc_output, 0, "Last controller output"); | &pc->pc_output, 0, "Last controller output"); | ||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "wghted avg output", CTLFLAG_RD, | |||||
&pc->pc_output_wavg, 0, "Last controller output"); | |||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "ticks", CTLFLAG_RD, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "ticks", CTLFLAG_RD, | ||||
&pc->pc_ticks, 0, "Last controller runtime"); | &pc->pc_ticks, 0, "Last controller runtime"); | ||||
SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "setpoint", CTLFLAG_RW, | SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "setpoint", CTLFLAG_RW, | ||||
Context not available. | |||||
return (output); | return (output); | ||||
} | } | ||||
static inline int div_round(int n, int d) | |||||
{ | |||||
return ((n > 0) ? (2*n + d) : (2*n - d)) / (2 * d); | |||||
} | |||||
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; | ||||
int delta; | |||||
error = pc->pc_setpoint - input; | error = pc->pc_setpoint - input; | ||||
/* | /* | ||||
Context not available. | |||||
pc->pc_output += output; | pc->pc_output += output; | ||||
pc->pc_input = input; | pc->pc_input = input; | ||||
/* Update integer error statistics, using Welford's method, | |||||
from "Efficient Hardware Calculation of Running Statistics" | |||||
by Bailey and Klaiber. */ | |||||
++pc->pc_err_count; | |||||
delta = pc->pc_error - pc->pc_err_mean; | |||||
pc->pc_err_mean_rem += delta; | |||||
pc->pc_err_var_rem += delta * delta - pc->pc_err_var; | |||||
delta = div_round(pc->pc_err_mean_rem, pc->pc_err_count); | |||||
pc->pc_err_mean += delta; | |||||
pc->pc_err_var_rem -= delta * | |||||
(2 * pc->pc_err_mean_rem - pc->pc_err_count * delta); | |||||
pc->pc_err_mean_rem -= pc->pc_err_count * delta; | |||||
if (pc->pc_err_count > 1) { | |||||
delta = div_round(pc->pc_err_var_rem, pc->pc_err_count - 1); | |||||
pc->pc_err_var += delta; | |||||
pc->pc_err_var_rem -= (pc->pc_err_count - 1) * delta; | |||||
} | |||||
/* Update exponential weighted moving average on pc_output. */ | |||||
delta = pc->pc_output - pc->pc_output_wavg; | |||||
pc->pc_output_wavg_rem += delta; | |||||
delta = div_round(pc->pc_output_wavg_rem, MIN(pc->pc_err_count, 100)); | |||||
pc->pc_output_wavg += delta; | |||||
pc->pc_output_wavg_rem -= MIN(pc->pc_err_count, 100) * delta; | |||||
return (output); | return (output); | ||||
} | } | ||||
Context not available. |