/*
 * mydsp.c
 *
 *  Created on: 2. 11. 2021
 *      Author: student
 */

#include "mydsp.h"
/*
 * fltLoPass_init: vypocita konstanty c0 a c1 z pozadovane casove konstanty a vzorkovaci periody
 */
void fltLoPass_init(fltLoPass_typedef *par)
{
	par->c0 = par->Ts / (par->Ts + par->tau);
	par->c1 = 1.0f - par->c0;
}

/*
 * regPID: funkce regulatoru PID
 */
float regPID(float ek, regPID_typedef *par)
{
	register float yp, yi, yd, y; // register urychlý výpočet umístěním promněnných do registrů
	// Proporcionalni slozka
	yp = ek * par->Kp;

	// Integracni slozka
	yi = ek * par->Ki + par->yik_1;

	// Derivacni slozka
	yd = (ek - par->ek_1) * par->Kd;

	// pamet pro derivacni slozku
	par->ek_1 = ek;

	// suma PID clenu
	y = yp + yi + yd;

	// horni omezeni limity
	if (par->lim_h < y)
		return par->lim_h;

	// spodni omezeni limity
	if (par->lim_l > y)
		return par->lim_l;

	// pamet integratoru
	par->yik_1 = yi;
	return y;
}

/*
 * regPID_init: funkce pro inicializaci regulatoru PID
 */
void regPID_init( regPID_typedef *par)
{
	par->Ki = par->Kp*par->Ts/par->Ti;
	par->Kd = par->Kp*par->Td/par->Ts;
}


/*
 * regPI: funkce regulatoru PI
 */
float regPI(float ek, regPI_typedef *par)
{
	register float yp, yi, y; // register urychlý výpočet umístěním promněnných do registrů
	// Proporcionalni slozka
	yp = ek * par->Kp;

	// Integracni slozka
	yi = ek * par->Ki + par->yik_1;

	// suma PID clenu
	y = yp + yi;

	// horni omezeni limity
	if (par->lim_h < y)
		return par->lim_h;

	// spodni omezeni limity
	if (par->lim_l > y)
		return par->lim_l;

	// pamet integratoru
	par->yik_1 = yi;
	return y;
}

/*
 * regPI_init: funkce pro inicializaci regulatoru PI
 */
void regPI_init( regPI_typedef *par)
{
	par->Ki = par->Kp*par->Ts/par->Ti; //par->Ki = par->Kp*par->Ts/par->Ti;
}
