/*
 * pi.h
 *
 * Created: 01/02/2021 21:36:11
 *  Author: Nick-XMG
 */ 
#ifndef BLDC_PI_H_
#define BLDC_PI_H_

// ----------------------------------------------------------------------
// 	history
// ----------------------------------------------------------------------
// 	01.02.2021 - initial Revision

// ----------------------------------------------------------------------
// 	header files
// ----------------------------------------------------------------------
#include <asf.h>
#include "libfixmath\fixmath.h"
// ----------------------------------------------------------------------
// 	#defines
// ----------------------------------------------------------------------

// ----------------------------------------------------------------------
// 	global variables
// ----------------------------------------------------------------------

#ifdef __cplusplus
extern "C" {
	#endif
	

typedef volatile struct
{
	volatile fix16_t Kp;          // the proportional gain for the PI controller
	volatile fix16_t Ki;          // the integral gain for the PI controller

	volatile fix16_t Ref_pu;      // the reference value [pu]
	volatile fix16_t Fbk_pu;      // the feedback value [pu]

	volatile fix16_t OutMax_pu;   // the saturation high limit for the controller output [pu]
	volatile fix16_t OutMin_pu;   // the saturation low limit for the controller output [pu]
	volatile fix16_t Out_pu;      // the controller output [pu]

	volatile fix16_t Ui;          // the integrator value for the PI controller
	volatile fix16_t error;          // the integrator value for the PI controller
	volatile bool SatFlag;   // flag to signal controller saturation
} PI_t;


// ----------------------------------------------------------------------
// 	functions
// ----------------------------------------------------------------------
// function to set default values for the object
inline void PI_objectInit(volatile PI_t *pPi_obj)
{
	PI_t *obj = (PI_t *)pPi_obj;

	// Function initializes the object with default values
	//fix16_t Kp = fix16_from_float(1.0);
	obj->Kp = 0;
	obj->Ki = 0;
	obj->Fbk_pu = 0;
	obj->Ref_pu = 0;
	obj->Out_pu = 0;
	obj->OutMax_pu = 0;
	obj->OutMin_pu = 0;
	obj->Ui = 0;
	obj->SatFlag = false;
}
// ----------------------------------------------------------------------

inline void PI_run(volatile PI_t *pPi_obj)
{
	PI_t *obj = (PI_t *)pPi_obj;
	volatile fix16_t up, preOut;

	// Compute the controller error
	obj->error = obj->Ref_pu - obj->Fbk_pu;

	// Compute the proportional term
	up = fix16_smul(obj->Kp, obj->error);

	// Compute the integral term in parallel form and saturate	
	obj->Ui = fix16_clamp(fix16_sadd(obj->Ui, fix16_smul(obj->Ki, up)), obj->OutMin_pu, obj->OutMax_pu);

	preOut = fix16_sadd(up, obj->Ui);

	// Saturate the output
 	obj->Out_pu = fix16_clamp(preOut, obj->OutMin_pu, obj->OutMax_pu);
	//obj->Out_pu = fix16_clamp(preOut, obj->OutMax_pu, -obj->OutMax_pu);

	// if saturation flag is needed, comment out:
 	obj->SatFlag = (obj->Out_pu == preOut) ? false : true;
}

// ----------------------------------------------------------------------
//
inline fix16_t PI_calcKp(float Ls_H, float deviceCurrent_A, float deviceVoltage_V,
float deviceCtrlPeriode_Sec)
{
	// calculation is based on "Betragsoptimum"
	// Kp = Ls/(2*tau)
	float x1;
	float y1;
	fix16_t Kp;

	// multiplication with deviceCurrent_A is to get per unit values
	x1 = (float)(Ls_H * deviceCurrent_A);

	y1 = (float)(2.0f * deviceCtrlPeriode_Sec);

	// multiplication with deviceVoltage_V is to get per unit values
	y1 = (float)(y1 * deviceVoltage_V);

	Kp = fix16_from_float(x1 / y1);
	return Kp;
}

// ----------------------------------------------------------------------
//
inline fix16_t PI_calcKi(float Rs_Ohm, float Ls_H, float deviceCtrlPeriode_Sec)
{
	// calculation is based on "TI - MotorWare's documentation"
	float RsByLs = (float)(Rs_Ohm / Ls_H);
	fix16_t Ki = fix16_from_float(RsByLs * deviceCtrlPeriode_Sec);
	//fix16_t Ki = 0;
	return Ki;
}

// ----------------------------------------------------------------------
// 	something...
// ----------------------------------------------------------------------

// ----------------------------------------------------------------------
// 	end of file
// ----------------------------------------------------------------------

#ifdef __cplusplus
}
#endif /* extern "C" */
#endif /* BLDC_PI_H_ */

