1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * pid.c PID controller for testing cooling devices |
4 | * |
5 | * Copyright (C) 2012 Intel Corporation. All rights reserved. |
6 | * |
7 | * Author Name Jacob Pan <jacob.jun.pan@linux.intel.com> |
8 | */ |
9 | |
10 | #include <unistd.h> |
11 | #include <stdio.h> |
12 | #include <stdlib.h> |
13 | #include <string.h> |
14 | #include <stdint.h> |
15 | #include <sys/types.h> |
16 | #include <dirent.h> |
17 | #include <libintl.h> |
18 | #include <ctype.h> |
19 | #include <assert.h> |
20 | #include <time.h> |
21 | #include <limits.h> |
22 | #include <math.h> |
23 | #include <sys/stat.h> |
24 | #include <syslog.h> |
25 | |
26 | #include "tmon.h" |
27 | |
28 | /************************************************************************** |
29 | * PID (Proportional-Integral-Derivative) controller is commonly used in |
30 | * linear control system, consider the process. |
31 | * G(s) = U(s)/E(s) |
32 | * kp = proportional gain |
33 | * ki = integral gain |
34 | * kd = derivative gain |
35 | * Ts |
36 | * We use type C Alan Bradley equation which takes set point off the |
37 | * output dependency in P and D term. |
38 | * |
39 | * y[k] = y[k-1] - kp*(x[k] - x[k-1]) + Ki*Ts*e[k] - Kd*(x[k] |
40 | * - 2*x[k-1]+x[k-2])/Ts |
41 | * |
42 | * |
43 | ***********************************************************************/ |
44 | struct pid_params p_param; |
45 | /* cached data from previous loop */ |
46 | static double xk_1, xk_2; /* input temperature x[k-#] */ |
47 | |
48 | /* |
49 | * TODO: make PID parameters tuned automatically, |
50 | * 1. use CPU burn to produce open loop unit step response |
51 | * 2. calculate PID based on Ziegler-Nichols rule |
52 | * |
53 | * add a flag for tuning PID |
54 | */ |
55 | int init_thermal_controller(void) |
56 | { |
57 | |
58 | /* init pid params */ |
59 | p_param.ts = ticktime; |
60 | /* TODO: get it from TUI tuning tab */ |
61 | p_param.kp = .36; |
62 | p_param.ki = 5.0; |
63 | p_param.kd = 0.19; |
64 | |
65 | p_param.t_target = target_temp_user; |
66 | |
67 | return 0; |
68 | } |
69 | |
70 | void controller_reset(void) |
71 | { |
72 | /* TODO: relax control data when not over thermal limit */ |
73 | syslog(LOG_DEBUG, "TC inactive, relax p-state\n" ); |
74 | p_param.y_k = 0.0; |
75 | xk_1 = 0.0; |
76 | xk_2 = 0.0; |
77 | set_ctrl_state(0); |
78 | } |
79 | |
80 | /* To be called at time interval Ts. Type C PID controller. |
81 | * y[k] = y[k-1] - kp*(x[k] - x[k-1]) + Ki*Ts*e[k] - Kd*(x[k] |
82 | * - 2*x[k-1]+x[k-2])/Ts |
83 | * TODO: add low pass filter for D term |
84 | */ |
85 | #define GUARD_BAND (2) |
86 | void controller_handler(const double xk, double *yk) |
87 | { |
88 | double ek; |
89 | double p_term, i_term, d_term; |
90 | |
91 | ek = p_param.t_target - xk; /* error */ |
92 | if (ek >= 3.0) { |
93 | syslog(LOG_DEBUG, "PID: %3.1f Below set point %3.1f, stop\n" , |
94 | xk, p_param.t_target); |
95 | controller_reset(); |
96 | *yk = 0.0; |
97 | return; |
98 | } |
99 | /* compute intermediate PID terms */ |
100 | p_term = -p_param.kp * (xk - xk_1); |
101 | i_term = p_param.kp * p_param.ki * p_param.ts * ek; |
102 | d_term = -p_param.kp * p_param.kd * (xk - 2 * xk_1 + xk_2) / p_param.ts; |
103 | /* compute output */ |
104 | *yk += p_term + i_term + d_term; |
105 | /* update sample data */ |
106 | xk_1 = xk; |
107 | xk_2 = xk_1; |
108 | |
109 | /* clamp output adjustment range */ |
110 | if (*yk < -LIMIT_HIGH) |
111 | *yk = -LIMIT_HIGH; |
112 | else if (*yk > -LIMIT_LOW) |
113 | *yk = -LIMIT_LOW; |
114 | |
115 | p_param.y_k = *yk; |
116 | |
117 | set_ctrl_state(lround(fabs(p_param.y_k))); |
118 | |
119 | } |
120 | |