MC100/Applications/foc/core/smo_observer.c

#include "app/nv_storage.h"
#include "math/fast_math.h"
#include "PMSM_FOC_Core.h"
#include "smo_observer.h"

#define USE_ARCTAN 1
static smo_observer_t smo;
static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta);
#ifdef USE_ARCTAN
static void smo_arctan(void);
#else
static void smo_pll(void);
#endif
void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta) {
	smo.ts = FOC_CTRL_US;
	smo.bandwith = pll_bandwith;
	smo.pll_kp = pll_bandwith * 2;
	smo.pll_ki = 0.25f * SQ(smo.pll_kp);
	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI * nv_get_motor_params()->poles;
	smo.lpf_wc = lpf_wc;
	smo.lpf_ceof = (lpf_wc*2*M_PI/(float)FOC_PWM_FS);
	smo.Ksmo = Ksmo;
	smo.Ksta = Ksta;
	smo.motor_r = nv_get_motor_params()->r;
	smo.motor_ld = nv_get_motor_params()->ld;
	smo.motor_lq = nv_get_motor_params()->lq;
	smo.motor_poles = nv_get_motor_params()->poles;
	smo.dir_ccw = true;
	smo.ldq_diff_dm = (smo.motor_ld-smo.motor_lq)/smo.motor_lq*smo.motor_ld;
	smo.ld_inv = 1.0f / smo.motor_ld;

	smo.pll.DT = smo.ts;
	smo.pll.kp = pll_bandwith * 2;
	smo.pll.ki = 0.25f * SQ(smo.pll.kp);
}	

float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta) {
	smo_observer(uAlpha, uBeta, iAlpha, iBeta);
#ifdef USE_ARCTAN
	smo_arctan();
#else
	smo_pll();
#endif
	return pi_2_degree(smo.est_angle_out);
}

smo_observer_t *get_smo(void) {
	return &smo;
}

static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
	float est_ab = smo.ldq_diff_dm * smo.est_rad_pers;//(smo.motor_ld-smo.motor_lq)/smo.motor_lq * smo.est_rad_pers;

	/* est alpha back emf */
	float calc_alpha = (uAlpha - smo.Ialpha_hat*smo.motor_r - smo.est_eAlpha - est_ab * smo.IBeta_hat) * smo.ld_inv;
	smo.Ialpha_hat += calc_alpha * smo.ts; //积分

	float err_iAlpha = smo.Ialpha_hat - iAlpha;
	
	smo.est_eAlpha = fclamp(err_iAlpha, -smo.Ksta, smo.Ksta) * smo.Ksmo;
	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);

	/* est beta back emf */
	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * smo.Ialpha_hat) * smo.ld_inv;
	smo.IBeta_hat += calc_beta * smo.ts; //积分
	
	float err_iBeta = smo.IBeta_hat - iBeta;
	
	smo.est_eBeta = fclamp(err_iBeta, -smo.Ksta, smo.Ksta) * smo.Ksmo;
	smo.est_eBeta_Filted = do_lpf(smo.est_eBeta_Filted, smo.est_eBeta, smo.lpf_ceof);

}
#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
#ifdef USE_ARCTAN
static void smo_arctan(void) {
	float ealpha_in = -smo.est_eAlpha_Filted;
	float ebeta_in  =  smo.est_eBeta_Filted;

	float angle = fast_atan_2(ealpha_in, ebeta_in); //通过反正切获取电角度
	angle_clamp(angle);
	float prev_angle = smo.est_angle;
	float comp_angle = 0.0f;
	if (smo.dir_ccw) {
		if (prev_angle > angle) {
			comp_angle = 2 * M_PI;
		}
	}else {
		if (prev_angle < angle) {
			comp_angle = -2 * M_PI;
		}
	}
	smo.est_angle = angle;
	smo.est_rad_pers = PLL_run(&smo.pll, angle, comp_angle);
	if (smo.est_rad_pers > smo.pll_max_rad_pers) {
		smo.est_rad_pers = smo.pll_max_rad_pers;
		smo.pll.out = smo.est_rad_pers;
	}
	smo.est_rad_pers_filted = smo.est_rad_pers;
	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
	angle_clamp(smo.est_angle_out);
	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
		smo.est_rpm = CONFIG_MAX_MOT_RPM;
	}else if (smo.est_rpm < 0) {
		smo.est_rpm = 0;
	}
}
#else
static void smo_pll(void) {
	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
	float ebeta_in  =  smo.est_eBeta_Filted/eab_sqr;
	float sin, cos;
	float angle_rad = pi_2_degree(smo.est_angle);
	
	arm_sin_cos_f32(angle_rad, &sin, &cos);
	float pi_err = cos * ealpha_in - sin * ebeta_in;
	float perr = pi_err * smo.pll_kp;
	smo.est_rad_pers = perr + smo.pll_integrator; //计算角速度
	if (smo.pll_integrator < smo.pll_max_rad_pers) {
		smo.pll_integrator += smo.ts * pi_err * smo.pll_ki; //更新pll的pi的积分xiang
	}
	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
	angle_clamp(smo.est_angle);
	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
		smo.est_rpm = CONFIG_MAX_MOT_RPM;
	}else if (smo.est_rpm < 0) {
		smo.est_rpm = 0;
	}
	/* 对低通进行角度补偿 */
	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
	//smo.est_angle_out = fast_atan_2(ealpha_in, ebeta_in) + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
	angle_clamp(smo.est_angle_out);
}
#endif

float smo_observer_est_angle(void) {
	return pi_2_degree(smo.est_angle_out);
}

//机械角度 rpm
float smo_observer_est_rpm(void) {
	return smo.est_rpm;
}