1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2017, Jeffrey Roberson <jeff@freebsd.org>
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 */
28
29 #include <sys/param.h>
30 #include <sys/kernel.h>
31 #include <sys/systm.h>
32 #include <sys/sysctl.h>
33 #include <sys/pidctrl.h>
34
35 void
pidctrl_init(struct pidctrl * pc,int interval,int setpoint,int bound,int Kpd,int Kid,int Kdd)36 pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound,
37 int Kpd, int Kid, int Kdd)
38 {
39
40 bzero(pc, sizeof(*pc));
41 pc->pc_setpoint = setpoint;
42 pc->pc_interval = interval;
43 pc->pc_bound = bound * setpoint * Kid;
44 pc->pc_Kpd = Kpd;
45 pc->pc_Kid = Kid;
46 pc->pc_Kdd = Kdd;
47 }
48
49 void
pidctrl_init_sysctl(struct pidctrl * pc,struct sysctl_oid_list * parent)50 pidctrl_init_sysctl(struct pidctrl *pc, struct sysctl_oid_list *parent)
51 {
52
53 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "error", CTLFLAG_RD,
54 &pc->pc_error, 0, "Current difference from setpoint value (P)");
55 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "olderror", CTLFLAG_RD,
56 &pc->pc_olderror, 0, "Error value from last interval");
57 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "integral", CTLFLAG_RD,
58 &pc->pc_integral, 0, "Accumulated error integral (I)");
59 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "derivative", CTLFLAG_RD,
60 &pc->pc_derivative, 0, "Error derivative (D)");
61 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "input", CTLFLAG_RD,
62 &pc->pc_input, 0, "Last controller process variable input");
63 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "output", CTLFLAG_RD,
64 &pc->pc_output, 0, "Last controller output");
65 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "ticks", CTLFLAG_RD,
66 &pc->pc_ticks, 0, "Last controller runtime");
67 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "setpoint", CTLFLAG_RW,
68 &pc->pc_setpoint, 0, "Desired level for process variable");
69 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "interval", CTLFLAG_RD,
70 &pc->pc_interval, 0, "Interval between calculations (ticks)");
71 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "bound", CTLFLAG_RW,
72 &pc->pc_bound, 0, "Integral wind-up limit");
73 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kpd", CTLFLAG_RW,
74 &pc->pc_Kpd, 0, "Inverse of proportional gain");
75 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kid", CTLFLAG_RW,
76 &pc->pc_Kid, 0, "Inverse of integral gain");
77 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kdd", CTLFLAG_RW,
78 &pc->pc_Kdd, 0, "Inverse of derivative gain");
79 }
80
81 int
pidctrl_classic(struct pidctrl * pc,int input)82 pidctrl_classic(struct pidctrl *pc, int input)
83 {
84 int output, error;
85 int Kpd, Kid, Kdd;
86
87 error = pc->pc_setpoint - input;
88 pc->pc_ticks = ticks;
89 pc->pc_olderror = pc->pc_error;
90
91 /* Fetch gains and prevent divide by zero. */
92 Kpd = MAX(pc->pc_Kpd, 1);
93 Kid = MAX(pc->pc_Kid, 1);
94 Kdd = MAX(pc->pc_Kdd, 1);
95
96 /* Compute P (proportional error), I (integral), D (derivative). */
97 pc->pc_error = error;
98 pc->pc_integral =
99 MAX(MIN(pc->pc_integral + error, pc->pc_bound), -pc->pc_bound);
100 pc->pc_derivative = error - pc->pc_olderror;
101
102 /* Divide by inverse gain values to produce output. */
103 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) +
104 (pc->pc_derivative / Kdd);
105 /* Save for sysctl. */
106 pc->pc_output = output;
107 pc->pc_input = input;
108
109 return (output);
110 }
111
112 int
pidctrl_daemon(struct pidctrl * pc,int input)113 pidctrl_daemon(struct pidctrl *pc, int input)
114 {
115 int output, error;
116 int Kpd, Kid, Kdd;
117
118 error = pc->pc_setpoint - input;
119 /*
120 * When ticks expires we reset our variables and start a new
121 * interval. If we're called multiple times during one interval
122 * we attempt to report a target as if the entire error came at
123 * the interval boundary.
124 */
125 if ((u_int)ticks - pc->pc_ticks >= pc->pc_interval) {
126 pc->pc_ticks = ticks;
127 pc->pc_olderror = pc->pc_error;
128 pc->pc_output = pc->pc_error = 0;
129 } else {
130 /* Calculate the error relative to the last call. */
131 error -= pc->pc_error - pc->pc_output;
132 }
133
134 /* Fetch gains and prevent divide by zero. */
135 Kpd = MAX(pc->pc_Kpd, 1);
136 Kid = MAX(pc->pc_Kid, 1);
137 Kdd = MAX(pc->pc_Kdd, 1);
138
139 /* Compute P (proportional error), I (integral), D (derivative). */
140 pc->pc_error += error;
141 pc->pc_integral =
142 MAX(MIN(pc->pc_integral + error, pc->pc_bound), 0);
143 pc->pc_derivative = pc->pc_error - pc->pc_olderror;
144
145 /* Divide by inverse gain values to produce output. */
146 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) +
147 (pc->pc_derivative / Kdd);
148 output = MAX(output - pc->pc_output, 0);
149 /* Save for sysctl. */
150 pc->pc_output += output;
151 pc->pc_input = input;
152
153 return (output);
154 }
155