Subversion Repositories cordic_atan_iq

 
function [angle, isqrt] = cordic_atan_iq(IS, QS);
 
IS = int32(IS);
QS = int32(QS);
 
tg = double(QS) / double(IS);
atg = atan(tg) / pi * 180;
fprintf('Given I = %g, Q = %g, angle = %g degree\n', IS, QS, atg);
 
sign = IS < 0;
if sign
    IS = -IS;
    QS = -QS;
end
 
% [F Exp_I] = log2(double(IS));
% [F Exp_Q] = log2(double(QS));
% 
% Exp_I
% Exp_Q
% 
% E = max(Exp_I, Exp_Q) + 1;
% 
% IS = int32(IS * 2^(31 - E));
% QS = int32(QS * 2^(31 - E));
 
fprintf('I = 0x%08x, Q = 0x%08x\n', IS, QS);
 
i = 0;
atan_table(1) = int32(1);
 
while i == 0 || atan_table(i) ~= 0
    atan_table(i+1) = int32(2^30 * atan(1/2^i) / (pi/2));
    i = i + 1;
end
 
k = 1;
for i=1:length(atan_table)
   k = k * 1 / sqrt(1 + 2^(-2 * (i-1)));
end
 
x(1) = IS;
y(1) = QS;
a(1) = int32(0);
 
tg = double(y(1)) / double(x(1));
atg = atan(tg) / pi * 180;
fprintf('i = %d: x = %d, y = %d (%g deg), a = %d(%g degree)\n', 0, x(1), y(1), atg, a(1), double(a(1))/2^30 * 90);
 
for i=2:length(atan_table) % i dont use last table element
    if y(i-1) > 0
        x(i) = x(i-1) + y(i-1) / 2^(i-2);
        y(i) = -x(i-1) / 2^(i-2) + y(i-1);
        a(i) = a(i-1) - atan_table(i-1);
        fprintf('i = %d: rot = %g deg, ', i-1, -double(atan_table(i-1)) / 2^30 * 90);
    else
        x(i) = x(i-1) - y(i-1) / 2^(i-2);
        y(i) = x(i-1) / 2^(i-2) + y(i-1);
        a(i) = a(i-1) + atan_table(i-1);
        fprintf('i = %d: rot = %g deg, ', i-1, double(atan_table(i-1)) / 2^30 * 90);
    end
 
    tg = double(y(i)) / double(x(i));
    atg = atan(tg) / pi * 180;    
    fprintf('x = %d, y = %d (%g deg), a = %d(%g deg)\n', x(i), y(i), atg, a(i), double(a(i))/2^30 * 90);
end
 
angle = -double(a(end))/2^30 * 90;
if sign
    if angle > 0
        angle = angle - 180;
    else
        angle = angle + 180;
    end
end
 
isqrt = int32(k * double(x(end)));
 
end