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