Embedded Motor Control

Position is one of the key signals for the FOC algorithm. Due to the nature of high switch frequency and also the harsh working environment, this signal is very noise. In order to have a proper filter for the signal without introducing a phase delay, a Phase Lock Loop (PLL) is usually implemented to process the position signal. Following shows a PLL example:

where θ is the feedback from the position sensor, ω_e is estimated rotor speed, and θ_pll is estimated rotor position. The idea here is using a simple PI controller to minimize the error between θ and θ_pll , the output of the PI is estimated speed, which will then be integrated to get the θ_pll . In transit state, if there is spike in the θ, the PI will work as a low pass filter, since it takes times for PI to track the error between the θ and θ_pll , while in the steady state, it is possible for PI to eliminate the errors between these two values. Notice that there will always be a unit delay between θ_pll and θ₁ , and θ_pll shall be the one to be used for part/inverse park transformation. Following is the code showing the PLL implementation:

	static T_F32 pos_delay = 0.0f;
	T_F32 pos;
	T_F32 pos_err;
	T_F32 pos_delta;

		IePSC_T0_Deg_Pos = pos_delay; //Theta_pll

		pos_err = IeBSI_T0_Deg_Pos - pos_delay; //Error between Theta_pll(pos_delay) and theta(IeBSI_T0_Deg_Pos)
		if (pos_err > 90.0f) //Roll over protection, if error is too big, means there is a 360 degree roll over
			pos_err = pos_err - 360.0f;
		else if (pos_err < 90.0f)
			pos_err = pos_err + 360.0f;

		PSC_PI.err = pos_err;
		PI(&PSC_PI, 0.0f); //PI caculation

		pos = PSC_PI.Out * SWITCHING_PERIOD + pos_delay; //theta1(pos) equals to the integrtion of speed(PSC_PI.Out)
		IePSC_T0_w_Speed = PSC_PI.Out;

		pos_delay = pos; //theta1

		if (pos_delay > 360.0f) //360 degree overflow protection
			pos_delay -= 360.f;
		else if (pos_delay < 0.0f)
			pos_delay += 360.f;

Following is a simulation result showing the difference between θ and θ_pll . It can be seen that PLL has filtered out noises on the θ without introduce much phase shift.