基于锁相环的角度观测器C语言实现

__ramfunc int32_t ATAN2_LT（int32_t axint32_t bx）
{
	int32_t EX_flagdx_changecx_tmp;
	int32_t ax_interbx_inter;

	ax_inter = ax;
	bx_inter = bx;
	EX_flag = 0;

	if（ax_inter == 0 && bx_inter == 0）
	{
		cx_tmp = 0;
		return cx_tmp;
	}

	if（ax_inter < 0）
	{
		ax_inter = -ax_inter;//????
		EX_flag = 1;
		//CALANG_1
	}

	//CALANG_1
	if（bx_inter < 0）
	{
		bx_inter = -bx_inter;
		EX_flag |= 0x2;
		//CALANG_2
	}

	//CALANG_2
	if（ax_inter < bx_inter）
	{
		dx_change = ax_inter;
		ax_inter = bx_inter;
		bx_inter = dx_change;//????
		EX_flag |= 0x4;
		//CALANG_3
	}

	//CALANG_3
	if（ax_inter != 0）
	{
		cx_tmp = （bx_inter * VALUE_Q15） / ax_inter;
		cx_tmp = atan0245_LT（cx_tmp）;
		if（（EX_flag & 0x4）==0x04）//bit2
		{
			cx_tmp = 0x4000 - cx_tmp;
			//CALANG_4
		}

		//CALANG_4
		EX_flag &= 3;
		if（EX_flag != 0）
		{
			if（EX_flag != 1）
			{
				//CALANG_5
				if（EX_flag != 3）
				{
					//CALANG_6
					cx_tmp = 0x10000 - cx_tmp;
					//CALANG_END
				}
				else
				{
					cx_tmp = 0x8000 + cx_tmp;
					//CALANG_END
				}
			}
			else
			{
				cx_tmp = 0x8000 - cx_tmp;
				//CALANG_END
			}
		}
	}
	//CALANG_END
	return cx_tmp;
}

//******************************************************************************
//电机1的转子角度估计
__ramfunc void motor_estimat_theta（void）
{
	const Q15 lambda = motor.Motor_PhiF_pu;
	const Q15 L = （（Q15_3div2）* motor.Motor_Lq_pu） / VALUE_Q15;//（（Q15_3div2）* motor.Motor_Lq_pu） / VALUE_Q15;
	const Q15 R = （（Q15_3div2）* motor.Motor_Rs_pu） / VALUE_Q15;//（（Q15_3div2）* motor.Motor_Rs_pu） / VALUE_Q15;

	Q15 ia_pu = motor_fbk.Ialpha_fbk_pu;
	Q15 ib_pu = motor_fbk.Ibeta_fbk_pu;
	Q15 U_alpha = motor_Estimate.Ualpha_pll_compens;
	Q15 U_beta = motor_Estimate.Ubeta_pll_compens;

	Q15 I_alpha_Diff = ia_pu - motor_fbk.I_alpha_pre;
	Q15 I_beta_Diff = ib_pu - motor_fbk.I_beta_pre;



	Q15 ia_Diff_ML = L * （ia_pu - motor_fbk.I_alpha_pre） / VALUE_Q15;
	Q15 ib_Diff_ML = L * （ib_pu - motor_fbk.I_beta_pre） / VALUE_Q15;

	motor_fbk.E_alpha = U_alpha - R * ia_pu / VALUE_Q15 - （ia_Diff_ML / DT） * VALUE_Q15;
	motor_fbk.E_beta = U_beta - R * ib_pu / VALUE_Q15 - （ib_Diff_ML / DT） * VALUE_Q15;

	motor_Estimate.E_alpha_filt = （motor_Estimate.E_alpha_filt * VALUE_Q15 + motor_Estimate.Klsf_Q15 *（motor_fbk.E_alpha - motor