xref: /freebsd/sys/kern/subr_pidctrl.c (revision 7fdf597e96a02165cfe22ff357b857d5fa15ed8a)
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
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
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
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
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