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 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 int ret = 0; 58 59 /* init pid params */ 60 p_param.ts = ticktime; 61 /* TODO: get it from TUI tuning tab */ 62 p_param.kp = .36; 63 p_param.ki = 5.0; 64 p_param.kd = 0.19; 65 66 p_param.t_target = target_temp_user; 67 68 return ret; 69 } 70 71 void controller_reset(void) 72 { 73 /* TODO: relax control data when not over thermal limit */ 74 syslog(LOG_DEBUG, "TC inactive, relax p-state\n"); 75 p_param.y_k = 0.0; 76 xk_1 = 0.0; 77 xk_2 = 0.0; 78 set_ctrl_state(0); 79 } 80 81 /* To be called at time interval Ts. Type C PID controller. 82 * y[k] = y[k-1] - kp*(x[k] - x[k-1]) + Ki*Ts*e[k] - Kd*(x[k] 83 * - 2*x[k-1]+x[k-2])/Ts 84 * TODO: add low pass filter for D term 85 */ 86 #define GUARD_BAND (2) 87 void controller_handler(const double xk, double *yk) 88 { 89 double ek; 90 double p_term, i_term, d_term; 91 92 ek = p_param.t_target - xk; /* error */ 93 if (ek >= 3.0) { 94 syslog(LOG_DEBUG, "PID: %3.1f Below set point %3.1f, stop\n", 95 xk, p_param.t_target); 96 controller_reset(); 97 *yk = 0.0; 98 return; 99 } 100 /* compute intermediate PID terms */ 101 p_term = -p_param.kp * (xk - xk_1); 102 i_term = p_param.kp * p_param.ki * p_param.ts * ek; 103 d_term = -p_param.kp * p_param.kd * (xk - 2 * xk_1 + xk_2) / p_param.ts; 104 /* compute output */ 105 *yk += p_term + i_term + d_term; 106 /* update sample data */ 107 xk_1 = xk; 108 xk_2 = xk_1; 109 110 /* clamp output adjustment range */ 111 if (*yk < -LIMIT_HIGH) 112 *yk = -LIMIT_HIGH; 113 else if (*yk > -LIMIT_LOW) 114 *yk = -LIMIT_LOW; 115 116 p_param.y_k = *yk; 117 118 set_ctrl_state(lround(fabs(p_param.y_k))); 119 120 } 121