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/cdefs.h> 30 #include <sys/param.h> 31 #include <sys/kernel.h> 32 #include <sys/systm.h> 33 #include <sys/sysctl.h> 34 #include <sys/pidctrl.h> 35 36 void 37 pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound, 38 int Kpd, int Kid, int Kdd) 39 { 40 41 bzero(pc, sizeof(*pc)); 42 pc->pc_setpoint = setpoint; 43 pc->pc_interval = interval; 44 pc->pc_bound = bound * setpoint * Kid; 45 pc->pc_Kpd = Kpd; 46 pc->pc_Kid = Kid; 47 pc->pc_Kdd = Kdd; 48 } 49 50 void 51 pidctrl_init_sysctl(struct pidctrl *pc, struct sysctl_oid_list *parent) 52 { 53 54 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "error", CTLFLAG_RD, 55 &pc->pc_error, 0, "Current difference from setpoint value (P)"); 56 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "olderror", CTLFLAG_RD, 57 &pc->pc_olderror, 0, "Error value from last interval"); 58 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "integral", CTLFLAG_RD, 59 &pc->pc_integral, 0, "Accumulated error integral (I)"); 60 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "derivative", CTLFLAG_RD, 61 &pc->pc_derivative, 0, "Error derivative (D)"); 62 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "input", CTLFLAG_RD, 63 &pc->pc_input, 0, "Last controller process variable input"); 64 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "output", CTLFLAG_RD, 65 &pc->pc_output, 0, "Last controller output"); 66 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "ticks", CTLFLAG_RD, 67 &pc->pc_ticks, 0, "Last controller runtime"); 68 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "setpoint", CTLFLAG_RW, 69 &pc->pc_setpoint, 0, "Desired level for process variable"); 70 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "interval", CTLFLAG_RD, 71 &pc->pc_interval, 0, "Interval between calculations (ticks)"); 72 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "bound", CTLFLAG_RW, 73 &pc->pc_bound, 0, "Integral wind-up limit"); 74 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kpd", CTLFLAG_RW, 75 &pc->pc_Kpd, 0, "Inverse of proportional gain"); 76 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kid", CTLFLAG_RW, 77 &pc->pc_Kid, 0, "Inverse of integral gain"); 78 SYSCTL_ADD_INT(NULL, parent, OID_AUTO, "kdd", CTLFLAG_RW, 79 &pc->pc_Kdd, 0, "Inverse of derivative gain"); 80 } 81 82 int 83 pidctrl_classic(struct pidctrl *pc, int input) 84 { 85 int output, error; 86 int Kpd, Kid, Kdd; 87 88 error = pc->pc_setpoint - input; 89 pc->pc_ticks = ticks; 90 pc->pc_olderror = pc->pc_error; 91 92 /* Fetch gains and prevent divide by zero. */ 93 Kpd = MAX(pc->pc_Kpd, 1); 94 Kid = MAX(pc->pc_Kid, 1); 95 Kdd = MAX(pc->pc_Kdd, 1); 96 97 /* Compute P (proportional error), I (integral), D (derivative). */ 98 pc->pc_error = error; 99 pc->pc_integral = 100 MAX(MIN(pc->pc_integral + error, pc->pc_bound), -pc->pc_bound); 101 pc->pc_derivative = error - pc->pc_olderror; 102 103 /* Divide by inverse gain values to produce output. */ 104 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) + 105 (pc->pc_derivative / Kdd); 106 /* Save for sysctl. */ 107 pc->pc_output = output; 108 pc->pc_input = input; 109 110 return (output); 111 } 112 113 int 114 pidctrl_daemon(struct pidctrl *pc, int input) 115 { 116 int output, error; 117 int Kpd, Kid, Kdd; 118 119 error = pc->pc_setpoint - input; 120 /* 121 * When ticks expires we reset our variables and start a new 122 * interval. If we're called multiple times during one interval 123 * we attempt to report a target as if the entire error came at 124 * the interval boundary. 125 */ 126 if ((u_int)ticks - pc->pc_ticks >= pc->pc_interval) { 127 pc->pc_ticks = ticks; 128 pc->pc_olderror = pc->pc_error; 129 pc->pc_output = pc->pc_error = 0; 130 } else { 131 /* Calculate the error relative to the last call. */ 132 error -= pc->pc_error - pc->pc_output; 133 } 134 135 /* Fetch gains and prevent divide by zero. */ 136 Kpd = MAX(pc->pc_Kpd, 1); 137 Kid = MAX(pc->pc_Kid, 1); 138 Kdd = MAX(pc->pc_Kdd, 1); 139 140 /* Compute P (proportional error), I (integral), D (derivative). */ 141 pc->pc_error += error; 142 pc->pc_integral = 143 MAX(MIN(pc->pc_integral + error, pc->pc_bound), 0); 144 pc->pc_derivative = pc->pc_error - pc->pc_olderror; 145 146 /* Divide by inverse gain values to produce output. */ 147 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) + 148 (pc->pc_derivative / Kdd); 149 output = MAX(output - pc->pc_output, 0); 150 /* Save for sysctl. */ 151 pc->pc_output += output; 152 pc->pc_input = input; 153 154 return (output); 155 } 156