atan2cordic.c

是codic算法实现atan的C程序

/* float programe */
#include "headDefine.h"

float atan2Cordic(float Y, float X) {
	
	int i;  				// for iteration
	int N = 14;  			// loop times
	//int ZoomIn = 15;		// theta is magnified by (2^15)

	float absX[14] = {0};
	float absY[14] = {0};
	
	float theta[14] = {0};
	double estiTheta = 0;	// the value of atan2(X, Y)
	
	if (X == 0 || Y == 0) {
		if (X ==0) {
			if (Y > 0) 
				estiTheta = PI / 2;
			else if(Y < 0)
				estiTheta = -PI / 2;
			else estiTheta = 0;
		}
		if (Y ==0) {
			if (X >= 0) 
				estiTheta = 0;
			else if(X < 0)
				estiTheta = PI;
		}
	}	
	else {
		// get the abs(X) and abs(Y)
		if (X >= 0) {
			absX[0] = X;
		}
		else absX[0] = -X;
			
		if (Y >= 0) {
			absY[0] = Y;
		}
		else absY[0] = -Y;
		
		
		// get the theta array
		for (i = 0; i < N; i++) {
			theta[i] = atan(1 / pow(2, i));
		}
		
		// main operation ------------------ 
		for (i = 0; i < N - 1; i++) {
			if (absY[i] > 0) {
				estiTheta = estiTheta + theta[i];
				absX[i + 1] = absX[i] + absY[i] / pow(2, i);
				absY[i + 1] = absY[i] - absX[i] / pow(2, i);
			}
			else if (absY[i] < 0) {
				estiTheta = estiTheta - theta[i];
				absX[i + 1] = absX[i] - absY[i] / pow(2, i);
				absY[i + 1] = absY[i] + absX[i] / pow(2, i);
			}
			else estiTheta = estiTheta;
		}
		
		// judge which quadrant estiTheta is 
		if (X > 0) {
			if (Y >0) {			// the first quadrant
				estiTheta = estiTheta;
			}
			else if (Y < 0) {	// the forth quadrant
				estiTheta = -estiTheta;
			}
		}
		else if (X < 0) {
			if (Y >0) {			// the second quadrant
				estiTheta = PI - estiTheta;
			}
			else if (Y < 0) {	// the third quadrant
				estiTheta = estiTheta - PI;
			}
		}
	}

	return(estiTheta);
}