LXR linux/tools/thermal/tmon/pid.c

   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 ***********************************************************************/
  55struct pid_params p_param;
  56/* cached data from previous loop */
  57static 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 */
  66int 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
  82void 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)
  98void 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}
 132