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