1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 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 __FBSDID("$FreeBSD$"); 31 32 #include <sys/param.h> 33 #include <sys/kernel.h> 34 #include <sys/systm.h> 35 #include <sys/sysctl.h> 36 #include <sys/pidctrl.h> 37 38 void 39 pidctrl_init(struct pidctrl *pc, int interval, int setpoint, int bound, 40 int Kpd, int Kid, int Kdd) 41 { 42 43 bzero(pc, sizeof(*pc)); 44 pc->pc_setpoint = setpoint; 45 pc->pc_interval = interval; 46 pc->pc_bound = bound * setpoint * Kid; 47 pc->pc_Kpd = Kpd; 48 pc->pc_Kid = Kid; 49 pc->pc_Kdd = Kdd; 50 } 51 52 void 53 pidctrl_init_sysctl(struct pidctrl *pc, struct sysctl_oid_list *parent) 54 { 55 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"); 82 } 83 84 int 85 pidctrl_classic(struct pidctrl *pc, int input) 86 { 87 int output, error; 88 int Kpd, Kid, Kdd; 89 90 error = pc->pc_setpoint - input; 91 pc->pc_ticks = ticks; 92 pc->pc_olderror = pc->pc_error; 93 94 /* Fetch gains and prevent divide by zero. */ 95 Kpd = MAX(pc->pc_Kpd, 1); 96 Kid = MAX(pc->pc_Kid, 1); 97 Kdd = MAX(pc->pc_Kdd, 1); 98 99 /* Compute P (proportional error), I (integral), D (derivative). */ 100 pc->pc_error = error; 101 pc->pc_integral = 102 MAX(MIN(pc->pc_integral + error, pc->pc_bound), -pc->pc_bound); 103 pc->pc_derivative = error - pc->pc_olderror; 104 105 /* Divide by inverse gain values to produce output. */ 106 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) + 107 (pc->pc_derivative / Kdd); 108 /* Save for sysctl. */ 109 pc->pc_output = output; 110 pc->pc_input = input; 111 112 return (output); 113 } 114 115 int 116 pidctrl_daemon(struct pidctrl *pc, int input) 117 { 118 int output, error; 119 int Kpd, Kid, Kdd; 120 121 error = pc->pc_setpoint - input; 122 /* 123 * When ticks expires we reset our variables and start a new 124 * interval. If we're called multiple times during one interval 125 * we attempt to report a target as if the entire error came at 126 * the interval boundary. 127 */ 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; 132 } else { 133 /* Calculate the error relative to the last call. */ 134 error -= pc->pc_error - pc->pc_output; 135 } 136 137 /* Fetch gains and prevent divide by zero. */ 138 Kpd = MAX(pc->pc_Kpd, 1); 139 Kid = MAX(pc->pc_Kid, 1); 140 Kdd = MAX(pc->pc_Kdd, 1); 141 142 /* Compute P (proportional error), I (integral), D (derivative). */ 143 pc->pc_error += error; 144 pc->pc_integral = 145 MAX(MIN(pc->pc_integral + error, pc->pc_bound), 0); 146 pc->pc_derivative = pc->pc_error - pc->pc_olderror; 147 148 /* Divide by inverse gain values to produce output. */ 149 output = (pc->pc_error / Kpd) + (pc->pc_integral / Kid) + 150 (pc->pc_derivative / Kdd); 151 output = MAX(output - pc->pc_output, 0); 152 /* Save for sysctl. */ 153 pc->pc_output += output; 154 pc->pc_input = input; 155 156 return (output); 157 } 158