1 files changed, 3 insertions, 91 deletions

diff --git a/test/sdr_fm.c b/test/sdr_fm.c
index a7f5551..4fc4c98 100644
--- a/test/sdr_fm.c
+++ b/test/sdr_fm.c
@@ -8,8 +8,10 @@
 
 #include <sys/timeb.h>
 
+#include <core/math.h>
 #include <hw/hw.h>
 #include <hw/sdr.h>
+#include <filt/filt.h>
 
 #define SAMPLE_RATE   1000000
 #define RESAMPLE_RATE 50000
@@ -149,99 +151,9 @@ uint8_t super_div20u8( uint16_t num )
 	return (uint8_t)r;
 }
 
-void rotate_90(uint8_t *buf, uint32_t len)
-/* 90 rotation is 1+0j, 0+1j, -1+0j, 0-1j
-   or [0, 1, -3, 2, -4, -5, 7, -6] */
-{
-	uint32_t i;
-	uint8_t tmp;
-	for (i=0; i<len; i+=8) {
-		/* uint8_t negation = 255 - x */
-		tmp = 255 - buf[i+3];
-		buf[i+3] = buf[i+2];
-		buf[i+2] = tmp;
-
-		buf[i+4] = 255 - buf[i+4];
-		buf[i+5] = 255 - buf[i+5];
-
-		tmp = 255 - buf[i+6];
-		buf[i+6] = buf[i+7];
-		buf[i+7] = tmp;
-	}
-}
-
-float to_float(uint8_t x) {
-    return (1.0f/127.0f)*(((float)x)-127.0f);
-}
 
-//float ph_ch( uint8_t i1, uint8_t q1, uint8_t i2, uint8_t q2)
-float ph_ch( uint8_t i1, uint8_t q1 )
-{
-	static float complex last=0.0+0.0i;
-	float out;
-	float complex xy,c1;
-	//float c2;
-
-	c1 = to_float(i1) + I*to_float(q1);
-	//c1 = CMPLXF( to_float(i1), to_float(q1) );
-	//c2 = to_float(i2) + I*to_float(q2);
-	//c2 = CMPLXF( to_float(i2), to_float(q2) );
-	xy = conjf(last)*c1;
-	out = cargf( xy );
-	last = c1;
-
-	return out;
-}
-
-//delay filtering
-void delay_filt( uint8_t *buf, int buf_len )
-{
-	//delay length
-	const int n=10;
-	
-	int i=0;
-	uint32_t cycle=0;
-	uint32_t avg_i=0, avg_q=0;
-	uint32_t cyc_buffer_i[n],delay_i=0;
-	uint32_t cyc_buffer_q[n],delay_q=0;
-	uint8_t in1=0,in2=0,out1=0,out2=0;
-
-	memset( cyc_buffer_i, 0, n*sizeof(uint32_t) );
-	memset( cyc_buffer_q, 0, n*sizeof(uint32_t) );
-
-	//for (i=0; i<(buf_len-(n*2));i+=2)
-	for (i=0 ; i<(buf_len-1) ; i+=2 )
-	//for (i=0; i<1000; i+=2)
-	{
-		in1 = buf[i];
-		in2 = buf[i+1];
 
-		//average
-		avg_i += (uint32_t)in1;
-		avg_q += (uint32_t)in2;
 
-		delay_i = cyc_buffer_i[cycle];
-		delay_q = cyc_buffer_q[cycle];
-		
-		cyc_buffer_i[cycle] = avg_i;
-		cyc_buffer_q[cycle] = avg_q;
-		cycle = cycle + 1;
-		if ( cycle >= n )
-		{
-			cycle = 0;
-		}
-
-		out1 = (avg_i - delay_i)/n;
-		out2 = (avg_q - delay_q)/n;
-
-		buf[i]   = out1;
-		buf[i+1] = out2;
-
-		//printf("%d,avg=[%d,%d],delay=[%d,%d],in=[%d,%d],out=[%d,%d]\n",
-		//	cycle, avg_i,avg_q,delay_i,delay_q,in1,in2,out1,out2);
-
-	}
-}
 
 int main( int argc, char **argv )
 {
@@ -334,7 +246,7 @@ int main( int argc, char **argv )
 		//	fbuf[i] = to_float(sample_buf[i]);
 		//}
 		//delay boxed filter
-		delay_filt( sample_buf, sample_len );
+		filt_delay( sample_buf, sample_len );
 
 
 		//rotate by 90 degrees uint8_t