120 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * pid.c PID controller for testing cooling devices4 *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 in30 * linear control system, consider the process.31 * G(s) = U(s)/E(s)32 * kp = proportional gain33 * ki = integral gain34 * kd = derivative gain35 * Ts36 * We use type C Alan Bradley equation which takes set point off the37 * 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])/Ts41 *42 *43 ***********************************************************************/44struct pid_params p_param;45/* cached data from previous loop */46static 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 response51 * 2. calculate PID based on Ziegler-Nichols rule52 *53 * add a flag for tuning PID54 */55int 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 70void 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])/Ts83 * TODO: add low pass filter for D term84 */85#define GUARD_BAND (2)86void 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