MC100/Applications/foc/core/PI_Controller.h

#ifndef _PI_Contrller_H__
#define _PI_Contrller_H__
#include "math/fix_math.h"
#include "bsp/bsp.h"
#if 0
typedef struct {
	s16q5_t  kp;
	s16q5_t  ki;
	s16q5_t  max;
	s16q5_t  min;
	s16q5_t  Ui;
	s16q14_t DT;
}PI_Controller;

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

static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
	s16q5_t kp_err = S16_mul(err,pi->kp, 5);
	s16q5_t ki_err = S16_mul(err,pi->ki, 5);
	s16q5_t integral = S16_mul(ki_err, pi->DT, 14);
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	s32q14_t out = pi->Ui + kp_err;
	return MATH_sat(out, pi->min, pi->max);
}
#else
typedef struct {
	float  kp;
	float  ki;
	float  max;
	float  min;
	float  Ui;
	float  DT;
}PI_Controller;

static __INLINE void PI_Controller_Init(PI_Controller *pi, float kp, float ki, float max, float min, float DT) {
	pi->kp = kp;
	pi->ki = ki;
	pi->max = max;
	pi->min = min;
	pi->Ui = 0;
	pi->DT = DT;
}
static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
	pi->Ui = (init);
}

static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);
	float ki_err = (err) * pi->ki;
	float integral = ki_err * pi->DT;
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	float out = pi->Ui + kp_err;
	return (MATH_sat(out, pi->min, pi->max));
}

#endif
#endif	/*_PI_Contrller_H__*/