| 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 | } |