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