Embedded Software and Motor Control Libraries for PXR40xx

Function GFLIB_AtanYXShifted_F32
// thoffsetdeg = angle offset (theta offset) in degrees
// NY - scaling coefficient of y signal
// NX - scaling coefficient of x signal
// KY - multiplication coefficient of y signal
// KX - multiplication coefficient of x signal
// THETAADJ - adjusting angle in radians, scaled from [-pi, pi) to [-1, 1)
return;
if (dthdeg < -180) || (dthdeg >= 180)
error('atanyxshiftedpar: dthdeg out of range');
end
if (thoffsetdeg < -180) || (thoffsetdeg >= 180)
error('atanyxshiftedpar: thoffsetdeg out of range');
end
dth2 = ((dthdeg/2)/180*pi);
thoffset = (thoffsetdeg/180*pi);
CY = (1 - 2^-15)/(2*cos(dth2));
CX = (1 - 2^-15)/(2*sin(dth2));
if(abs(CY) >= 1) NY = ceil(log2(abs(CY)));
else NY = 0;
end
if(abs(CX) >= 1) NX = ceil(log2(abs(CX)));
else NX = 0;
end
KY = CY/2^NY;
KX = CX/2^NX;
THETAADJ = dthdeg/2 - thoffsetdeg;
if THETAADJ >= 180
THETAADJ = THETAADJ - 360;
elseif THETAADJ < -180
THETAADJ = THETAADJ + 360;
end
THETAADJ = THETAADJ/180;
At some values of the phase shift, and particularly at phase shift approaching -180, 0 or
180 degrees, the algorithm may become numerically unstable, causing any error,
contributed by input signal imperfections or through finite precision arithmetic, to be
magnified significantly. Therefore, some care should be taken to avoid error where
possible. The detailed error analysis of the algorithm is beyond the scope of this
documentation, however, general guidelines are provided.
There are several sources of error in the function:
• error of the supplied signal values due to the finite resolution of the AD conversion
• error contributed by higher order harmonics appearing in the input signals
• computational error of the multiplication due to the finite length of registers
• error of the phase shift Δθ representation in the finite precision arithmetic and in the
values
• error due to differences in signal amplitudes
It should be noted that the function requires both signals to have the same amplitude. To
minimize the output error, the amplitude of both signals should be as close to 1.0 as much
as possible.
Embedded Software and Motor Control Libraries for PXR40xx, Rev. 1.0
256 Freescale Semiconductor, Inc.