33#include <sys/kernel.h>
35#include <sys/sysctl.h>
36#include <sys/pidctrl.h>
39pidctrl_init(
struct pidctrl *pc,
int interval,
int setpoint,
int bound,
40 int Kpd,
int Kid,
int Kdd)
43 bzero(pc,
sizeof(*pc));
44 pc->pc_setpoint = setpoint;
45 pc->pc_interval = interval;
46 pc->pc_bound = bound * setpoint * Kid;
56 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"error", CTLFLAG_RD,
57 &pc->pc_error, 0,
"Current difference from setpoint value (P)");
58 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"olderror", CTLFLAG_RD,
59 &pc->pc_olderror, 0,
"Error value from last interval");
60 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"integral", CTLFLAG_RD,
61 &pc->pc_integral, 0,
"Accumulated error integral (I)");
62 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"derivative", CTLFLAG_RD,
63 &pc->pc_derivative, 0,
"Error derivative (D)");
64 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"input", CTLFLAG_RD,
65 &pc->pc_input, 0,
"Last controller process variable input");
66 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"output", CTLFLAG_RD,
67 &pc->pc_output, 0,
"Last controller output");
68 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"ticks", CTLFLAG_RD,
69 &pc->pc_ticks, 0,
"Last controller runtime");
70 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"setpoint", CTLFLAG_RW,
71 &pc->pc_setpoint, 0,
"Desired level for process variable");
72 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"interval", CTLFLAG_RD,
73 &pc->pc_interval, 0,
"Interval between calculations (ticks)");
74 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"bound", CTLFLAG_RW,
75 &pc->pc_bound, 0,
"Integral wind-up limit");
76 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"kpd", CTLFLAG_RW,
77 &pc->pc_Kpd, 0,
"Inverse of proportional gain");
78 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"kid", CTLFLAG_RW,
79 &pc->pc_Kid, 0,
"Inverse of integral gain");
80 SYSCTL_ADD_INT(NULL,
parent, OID_AUTO,
"kdd", CTLFLAG_RW,
81 &pc->pc_Kdd, 0,
"Inverse of derivative gain");
90 error = pc->pc_setpoint - input;
92 pc->pc_olderror = pc->pc_error;
95 Kpd = MAX(pc->pc_Kpd, 1);
96 Kid = MAX(pc->pc_Kid, 1);
97 Kdd = MAX(pc->pc_Kdd, 1);
100 pc->pc_error = error;
102 MAX(MIN(pc->pc_integral + error, pc->pc_bound), -pc->pc_bound);
103 pc->pc_derivative = error - pc->pc_olderror;
106 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) +
107 (pc->pc_derivative / Kdd);
109 pc->pc_output = output;
110 pc->pc_input = input;
121 error = pc->pc_setpoint - input;
128 if ((u_int)
ticks - pc->pc_ticks >= pc->pc_interval) {
129 pc->pc_ticks =
ticks;
130 pc->pc_olderror = pc->pc_error;
131 pc->pc_output = pc->pc_error = 0;
134 error -= pc->pc_error - pc->pc_output;
138 Kpd = MAX(pc->pc_Kpd, 1);
139 Kid = MAX(pc->pc_Kid, 1);
140 Kdd = MAX(pc->pc_Kdd, 1);
143 pc->pc_error += error;
145 MAX(MIN(pc->pc_integral + error, pc->pc_bound), 0);
146 pc->pc_derivative = pc->pc_error - pc->pc_olderror;
149 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) +
150 (pc->pc_derivative / Kdd);
151 output = MAX(output - pc->pc_output, 0);
153 pc->pc_output += output;
154 pc->pc_input = input;
void pidctrl_init_sysctl(struct pidctrl *pc, struct sysctl_oid_list *parent)
int pidctrl_daemon(struct pidctrl *pc, int input)
void pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound, int Kpd, int Kid, int Kdd)
int pidctrl_classic(struct pidctrl *pc, int input)