Working with phasor re-rotation

ch32v/ch32v203-goertzel/ttest/Makefile

@@ -1,4 +1,10 @@
-all : test floattest
+all : test floattest phasor_rotation_test
+
+phasor_rotation_testt : phasor_rotation_test.c
+	gcc -o $@ $^ -lm -g
+
+phasor_rotation_test : phasor_rotation_testt
+	./phasor_rotation_testt
 
 floattest : floattestt
 	./floattestt

ch32v/ch32v203-goertzel/ttest/floattestt.c

@@ -0,0 +1,44 @@
+#include <stdio.h>
+#include <math.h>
+
+int main()
+{
+	const float omegaPerSample = 3.1415926*2.0 / 128; // pi / 200
+
+	// Only divide by 128 to show two cycles.
+
+	const int numSamples = 256; // enough to go from 0 to 2pi
+
+	float phase = 0;
+	//for( float phase = 0; phase < 3.1415926*2.0; phase += 0.01 )
+	{
+		float coeff = 2 * cos( omegaPerSample );
+		int i;
+
+		// TRICKY: When you want a sinewave, initialize with omegaPerSample.  This
+		// is crucial.  The initial state will have massive consequences.
+		float sprev = omegaPerSample;
+		float sprev2 = 0;
+		for( i = 0; i < numSamples; i++ )
+		{
+			// If you wanted to do a DFT, set SAMPLE to your incoming sample.
+			float SAMPLE = 65536 * sin( phase + i * omegaPerSample );
+
+			// Here is where the magic happens.
+			float s = SAMPLE + coeff * sprev - sprev2;
+			sprev2 = sprev;
+			sprev = s;
+
+		// For DFT, your power will be:
+		float power = sprev*sprev + sprev2*sprev2 - (coeff * sprev * sprev2);
+		//printf( "Power: %f\n", power );
+
+		float coeff_s = 2 * sin( omegaPerSample );
+		double rR = 0.5 * coeff   * sprev - sprev2;
+		double rI = 0.5 * coeff_s * sprev;
+		printf( "%d,%f,%f\n", i, rR, rI);
+
+		}
+
+	}
+}

ch32v/ch32v203-goertzel/ttest/phasor_rotation_test.c

@@ -0,0 +1,43 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+
+int main()
+{
+	int phasor_r = 32768;
+	int phasor_i = 0;
+	double phasor = 0;
+
+	double omega = 0.1;
+	int omega_r = cos( omega ) * 32768;
+	int omega_i = sin( omega ) * 32768;
+
+	int i;
+	for( i = 0; i < 10000; i++ )
+	{
+		
+		int32_t temp = (phasor_r * omega_i + phasor_i * omega_r ) >> 15;
+		phasor_r = (phasor_r * omega_r - phasor_i * omega_i ) >> 15;
+		phasor_i = temp;
+		phasor += omega;
+
+
+		// Approximate sqrt(x*x+y*y)
+		#define ABS(x) (((x)<0)?-(x):(x))
+		int s = phasor_r * phasor_r + phasor_i * phasor_i;
+		int intensity = (ABS(phasor_r) + ABS(phasor_i)) * 26100 / 32768 + 1; // Found experimentally (Also try to avoid divide-by-zero.
+		intensity = (intensity + s/intensity)/2;
+		intensity = (intensity + s/intensity)/2;
+
+		if( intensity < 32763 )
+		{
+			phasor_r += phasor_r >> 12;
+			phasor_i += phasor_i >> 12;
+		}
+
+		double fA = atan2( phasor_i, phasor_r );
+		printf( "%6d %6d / %6d %6d / %d / %f %f %f\n", omega_r, omega_i, phasor_r, phasor_i, intensity, fA, phasor, fA-phasor );
+		if( phasor >= 3.141592653589 ) phasor -= 3.141592653589*2;
+	}
+
+}