CORDIC

cordic_tests.c

/*
 ============================================================================
 Name        : SinCosProject.c
 Author      : 
 Version     :
 Copyright   : Your copyright notice
 Description : Hello World in C, Ansi-style
 ============================================================================
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

//Cordic in 32 bit signed fixed point math
//Function is valid for arguments in range -pi/2 -- pi/2
//for values pi/2--pi: value = half_pi-(theta-half_pi) and similarly for values -pi---pi/2
//
// 1.0 = 1073741824
// 1/k = 0.6072529350088812561694
// pi = 3.1415926535897932384626
//Constants
#define cordic_1K 0x26DD3B6A
#define half_pi 0x6487ED51
#define MUL 1073741824.000000
#define MUL_RECIP 1.0/MUL
#define CORDIC_NTAB 32
int cordic_ctab[] = { 0x3243F6A8, 0x1DAC6705, 0x0FADBAFC, 0x07F56EA6,
		0x03FEAB76, 0x01FFD55B, 0x00FFFAAA, 0x007FFF55, 0x003FFFEA, 0x001FFFFD,
		0x000FFFFF, 0x0007FFFF, 0x0003FFFF, 0x0001FFFF, 0x0000FFFF, 0x00007FFF,
		0x00003FFF, 0x00001FFF, 0x00000FFF, 0x000007FF, 0x000003FF, 0x000001FF,
		0x000000FF, 0x0000007F, 0x0000003F, 0x0000001F, 0x0000000F, 0x00000008,
		0x00000004, 0x00000002, 0x00000001, 0x00000000, };

void cordic(int theta, int *s, int *c, int n) {
	int k, d, tx, ty, tz;
	int x = cordic_1K, y = 0, z = theta;
	n = (n > CORDIC_NTAB) ? CORDIC_NTAB : n;
	for (k = 0; k < n; ++k) {
		d = z >> 31;
		//get sign. for other architectures, you might want to use the more portable version
		//d = z>=0 ? 0 : -1;
		tx = x - (((y >> k) ^ d) - d);
		ty = y + (((x >> k) ^ d) - d);
		tz = z - ((cordic_ctab[k] ^ d) - d);
		x = tx;
		y = ty;
		z = tz;
	}
	*c = x;
	*s = y;
}

float normalizeRadiansToPlusMinusM_PI(float radians) {
	int sign = radians < 0 ? -1 : 1;
	radians = sign * radians;
	int revolutions = (int) (radians * M_1_PI) + 1;
	revolutions = (revolutions >> 1) << 1;

	radians = radians - revolutions * M_PI;
	return sign * radians;
}

int radiansToPlusMinusM_PI_2(float *radians) {
	int flip = 0;
	*radians = normalizeRadiansToPlusMinusM_PI(*radians);

	if (*radians < -M_PI_2 || *radians > M_PI_2) {
		if (*radians < 0) {
			*radians += M_PI;
		} else {
			*radians -= M_PI;
		}
		flip = 1; //flip the sign for second or third quadrant
	}
	return flip;
}

void cordicF(float theta, float *s, float *c, int n) {
	int sint, cint;
	int flip = radiansToPlusMinusM_PI_2(&theta);
	cordic(theta * MUL, &sint, &cint, n);
	*s = sint * MUL_RECIP;
	*c = cint * MUL_RECIP;
	if (flip) {
		*s = -*s;
		*c = -*c;
	}
}

void p_sincos_f32(const float *a, float *c, float *z, int n) {
	int i;
	for (i = 0; i < n; i++) {
		const float angle = a[i];
		float *pcos = z + i;
		float *psin = c + i;
		cordicF(angle, psin, pcos, 17);
	}
}

int main(void) {
	int angle;
	int n;
	for (n = 10; n <= 30; n++) {
		float maxError = 0;
		int r;
		int repeats = 10;
		int max = 72000;
		int min = -72000;
		for (angle = min; angle <= max; angle++) {
			float rads = angle * M_PI / 18000.0;
			float sinF, cosF, csinF, ccosF, dc, ds;
			cordicF(rads, &sinF, &cosF, n);
			csinF = sinf(rads);
			ccosF = cosf(rads);
			dc = ccosF - cosF;
			ds = csinF - sinF;
			if (dc < 0)
				dc = -dc;
			if (ds < 0)
				ds = -ds;
			//printf("Angle %f, Sin %.4f, Cos %.4f, Sin %.4f, Cos %.4f Delta %.6f/%.6f\n",
			//angle / 100.0, sinF, cosF, csinF, ccosF, ds, dc);
			maxError = dc > maxError ? dc : maxError;
			maxError = ds > maxError ? ds : maxError;
//			if (dc > error || ds > error) {
//				printf("ERROR %f/%f", ds, dc);
//				break;
//			}
		}
		{
			int range = max - min + 1;
			clock_t begin, end;
			double time_spent;
			begin = clock();
			for (r = 0; r < repeats; r++)
				for (angle = min; angle <= max; angle++) {
					float rads = angle * M_PI / 18000.0;
					float sinF, cosF;
					cordicF(rads, &sinF, &cosF, n);
				}
			end = clock();
			time_spent = (double) (end - begin) / CLOCKS_PER_SEC;

			printf("n=%d |max error|=%.8f in %f microseconds\n", n, maxError,
					1000000 * time_spent / (repeats * range));
		}
	}

	{
		float a[4] = { 0, M_PI, M_PI_2, M_PI_4 };
		float c[4];
		float z[4];
		int i;
		p_sincos_f32(&a[0], &c[0], &z[0], 4);
		for (i = 0; i < 4; i++)
			printf("X %.4f = s:%.4f (%.4f), c:%.4f (%.4f)\n", a[i], c[i],
					sinf(a[i]), z[i], cosf(a[i]));

	}

	return EXIT_SUCCESS;
}