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