void controller_handler(const double xk, double *yk)
{
double ek;
double p_term, i_term, d_term;
ek = p_param.t_target - xk; /* error */
if (ek >= 3.0) {
syslog(LOG_DEBUG, "PID: %3.1f Below set point %3.1f, stop\n",
xk, p_param.t_target);
controller_reset();
*yk = 0.0;
return;
}
/* compute intermediate PID terms */
p_term = -p_param.kp * (xk - xk_1);
i_term = p_param.kp * p_param.ki * p_param.ts * ek;
d_term = -p_param.kp * p_param.kd * (xk - 2 * xk_1 + xk_2) / p_param.ts;
/* compute output */
*yk += p_term + i_term + d_term;
/* update sample data */
xk_1 = xk;
xk_2 = xk_1;
/* clamp output adjustment range */
if (*yk < -LIMIT_HIGH)
*yk = -LIMIT_HIGH;
else if (*yk > -LIMIT_LOW)
*yk = -LIMIT_LOW;
p_param.y_k = *yk;
set_ctrl_state(lround(fabs(p_param.y_k)));
}
static void tmon_cleanup(void)
{
syslog(LOG_INFO, "TMON exit cleanup\n");
fflush(stdout);
refresh();
if (tmon_log)
fclose(tmon_log);
if (event_tid) {
pthread_mutex_lock(&input_lock);
pthread_cancel(event_tid);
pthread_mutex_unlock(&input_lock);
pthread_mutex_destroy(&input_lock);
}
closelog();
/* relax control knobs, undo throttling */
set_ctrl_state(0);
keypad(stdscr, FALSE);
echo();
nocbreak();
close_windows();
endwin();
free_thermal_data();
exit(1);
}
void controller_reset(void)
{
/* TODO: relax control data when not over thermal limit */
syslog(LOG_DEBUG, "TC inactive, relax p-state\n");
p_param.y_k = 0.0;
xk_1 = 0.0;
xk_2 = 0.0;
set_ctrl_state(0);
}
