Phase angle calculation with high accuracy 高精度相角计算
Two schemes for the phase angle/frequency calculation are implemented in my one of projects. One scheme is the zero-crossing in the time domain. Another is based on DFT in the frequency domain. The first one will be demonstrated later.
First of all, let's consider two sinusoidal waveforms with the same frequency. Two vectors can be obtained after taking DFT on those two waveforms. Assume the two vectors are represented as the following:
V1 = R1 + j I1 , V2 = R2 + j I2
Then the phase angle difference between V1 and V2 can be derived from the following equations:
|V1| * |V2| * exp(j (A1 - A2))
= |V1|*exp(j A1) * |V2|*exp(- j A2)
= (R1 + j I1) * (R2 - j I2)
= (R1 * R2 + I1 * I2) + j (R2 * I1 - R1 * I2)
where A1 and A2 are phase angles of the corresponding vectors.
Thus, the angle difference A1 - A2 = arctan2( (R1 * R2 + I1 * I2), (R2 * I1 - R1 * I2))
It is easy for some applications to compute the A1-A2 because there exists the arctan2 function in their library. But it could be a nightmare for some slow CPUs without the floating-point co-processor. A faster function should be developed by using fixed-point instructions.
The arctan function can be derived from the Taylor serial:
arctan(x) = 0.463647609000806 + 0.8*(x-0.5) -0.32*(x-0.5)^2 -0.0426666667*(x-0.5)^3 + 0.1536*(x-0.5)^4 -0.077824*(x-0.5)^5
The reason why x-0.5 is used is to get a more accurate result while x is always kept less than 1. PI is mapped into the number 32768 which can benefit from the periodical 16-bit signed or unsigned integer. For example: 2PI --> 2*32768 = 0x10000 = 0 (16-bit unsigned) or PI = -PI --> 32768 = -32768 (16-bit signed)
The source code (CPU: TMS320C5X)
;************************************************************************************************** ; Calculate arctan2 ;************************************************************************************************** ; In: ACC + j ACCB ; Out: ACC [0, 7FFFH] <--> [0, PI], [8000H, FFFFH] <--> [-PI, 0] ; ACC [0, FFFFH] <--> [0, 2PI] ;************************************************************************************************** Arctan2 ldp #temp0 sacl temp0 ;save real sach temp1 abs exar sacl temp2 ;save image sach temp3 abs sbb ;image - real bcnd _Use_arccotan, GT ;if real < image then use arcctan bcnd _arctan_1, LT lacc #0FFFFH and #0FFFFH b _arctan_2 _arctan_1 lacl temp2 ;accb = real addh temp3 ;acc = image abs exar ;acc = R, accb = I lar ar7, #17-1 call Div_32_32, ar7 ;ACCB = ACCB / ACC <---> image / real lacb _arctan_2 call Arctan, ar0 b _Arctan_Calc_1 _Use_arccotan lacl temp2 addh temp3 abs ; exar ;acc = R, accb = I lar ar7, #17-1 call Div_32_32, ar7 ;ACCB = ACCB / ACC <---> real / image lacb call Arctan, ar0 sacb lacc #7FFFH, 2 ;PI/2. FFFFH is PI/4 sbb ;arctan(real/image) = PI/2 - arctan(image/real) _Arctan_Calc_1 ldp #temp0 bsar 3 sacb ;[0, PI/2] <--> [0, 3FFFH] lacc temp1 bcnd _sect_23, LT lacc temp3 bcnd _sect_4, LT lacb ;[0, 3FFFH] ret _sect_4 lacb neg ret ;[C000H, FFFFH] _sect_23 lacc temp3 bcnd _sect_3, LT lacc #7FFFH sbb ret ;[4000H, 7FFFH] _sect_3 lacc #8000H and #0FFFFH addb ret ;[8000H, BFFFH] ;************************************************************************************************** ; Taylor Serial Expansion of arctan at 0.5: ; arctan(x) = 0.463647609000806 ; + 0.8 (x-0.5) ; + -0.32 (x-0.5)^2 ; + -0.0426666667 (x-0.5)^3 ; + 0.1536 (x-0.5)^4 ; + -0.077824 (x-0.5)^5 ;************************************************************************************************** ; arctan(x) = B0 + y*(B1 + y*(B2 + y*(B3 + y*(B4 + y*B5)))) ; y = x - 0.5 ; ; X : [0, 1.0] ---- [0, 2^16] ; X-0.5: [-0.5, 0.5] ---- [-2^15, 2^15] ; ; Result in ACC: [0, PI/4] <-----> [0, FFFFH] ; Max Error at 0 and 1.0 <---> 0, 45 degree : 0.27 degree ; Max Error after Correction: 0.05 degree ;************************************************************************************************** Coeff_B0 .SET 995675659 ;0.463647609000806 * 65536 * 32768 Coeff_B1 .SET 26214 Coeff_B2 .SET -10485 -80 ;with Correction Coeff Coeff_B3 .SET -1398 +80 Coeff_B4 .SET 5033 Coeff_B5 .SET -2550 Coeff_1_Over_PI .SET 41721/2 ;2* 2^16 / 3.1415927 ;************************************************************************************************** Arctan ldp #0 sub #7FFFH sub #1 sacl AR0 lt AR0 mpy #Coeff_B5 pac bsar 16 add #Coeff_B4 sacl AR1 mpy AR1 pac bsar 16 add #Coeff_B3 sacl AR1 mpy AR1 pac bsar 16 add #Coeff_B2 sacl AR1 mpy AR1 pac bsar 16 add #Coeff_B1 sacl AR1 mpy AR1 pac add #(Coeff_B0 >> 15), 15 add #(Coeff_B0 % 32768) abs sach ar0, 1 lacc #Coeff_1_Over_PI sacl TREG0 mpyu ar0 pac add #1, 13 bsar 14 ret