/*
 * ul_pid.c
 *
 *  Created on: Mar 26, 2012
 *      Author: qwer1
 */

#include "ul_pid.h"

void pid_reset(struct pid_struct *pid, S32 val) {
	pid->val = val;
	pid->e = pid->set - val;
	pid->i_cnt = 0;
	pid->i_sum = (S32)pid->e * pid->i_n;
	for (U8 i=0; i<PID_I_N; i++)
		pid->i_buf[i] = pid->e;
}

void pid_init(
	struct pid_struct *pid, S16 kp, S16 ki, S16 kd,
	U8 i_n, S32 out_min, S32 out_max, S16 out_div, S16 out_mul, S32 set, S32 val
) {
	pid->kp  = kp;
	pid->ki  = ki;
	pid->kd  = kd;
	pid->i_n = i_n;
	pid->out_min = out_min;
	pid->out_max = out_max;
	pid->out_div = out_div;
	pid->out_mul = out_mul;
	pid->set = set;
	pid_reset(pid, val);
}

S32 pid_update(struct pid_struct *pid, S32 val) {

	//calculate new errors
	S32 e    = pid->set - val;
	pid->de  = pid->e - e;
	pid->e   = e;
	pid->val = val;

	//update integral sum
	pid->i_sum = pid->i_sum - pid->i_buf[pid->i_cnt] + e;
	pid->i_buf[pid->i_cnt] = e;
	if (++(pid->i_cnt) >= pid->i_n) pid->i_cnt = 0;

	//calculate new OUT value
	pid->out = ((pid->out + pid->kp * e + pid->ki * pid->i_sum / pid->i_n + pid->kd * pid->de) * (S32)pid->out_mul) / (S32)pid->out_div;
	if (pid->out < pid->out_min) pid->out = pid->out_min;
	if (pid->out > pid->out_max) pid->out = pid->out_max;

	return pid->out;
}
