/*
 * pid.c
 *
 *  Created on: Mar 21, 2012
 *      Author: qwer1
 */

#include "pid.h"
#include "config.h"

struct pid_struct pids[T_TC_NUM];

void setup_pid(void) {
	pids_init();
}

void pids_init(void) {
	struct config_pid_struct *pid_c = &config.pid_c[0]; //1st PID parameters structure
	for (U8 i=0; i<T_TC_NUM; i++) { //set PID parameters
		if (pid_c->ni == 0) pid_c->ni = 1;
		pid_init(&pids[i], pid_c->kp, pid_c->ki, pid_c->kd, pid_c->ni, 0, PID_OUT_MAX, 1000, 1, 0, 0);
		if (t_is_ok(t_tc[i]))
			pid_reset(&pids[i], t_tc[i] * 100L);
		pid_c++;
	}
}

void pids_update(void) {
	for (U8 i=0; i<T_TC_NUM; i++) {
	    //for Tset<=50 set power 0..100% directly
		if (process.t_set_current[i] <= 50) {
    		pids[i].out = (process.t_set_current[i] * (pids[i].out_max - pids[i].out_min)) / 50L + pids[i].out_min;
		} else if (t_is_ok(t_tc[i])) {
			pids[i].set = process.t_set_current[i];
			pid_update(&pids[i], t_tc[i] * 100L);
		}
	}
}
