#ifndef _PI_Contrller_H__
#define _PI_Contrller_H__
#include "math/fix_math.h"
typedef struct {
	s32q14_t  kp;
	s32q14_t  ki;
	s32q14_t  max;
	s32q14_t  min;
	s32q14_t  Ui;
}PI_Controller;

static __INLINE void PI_Controller_Reset(PI_Controller *pi, s32q14_t init) {
	pi->Ui = init;
}

static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s32q14_t err) {
	s32q14_t kp_err = S32Q14_MUL(err,pi->kp);
	pi->Ui = MATH_sat(pi->Ui + S32Q14_MUL(kp_err,pi->ki), pi->min, pi->max);
	s32q14_t out = pi->Ui + kp_err;
	return MATH_sat(out, pi->min, pi->max) >> 9;
}

#endif	/*_PI_Contrller_H__*/