diff --git a/ch32v/ch32v203-goertzel/adcgoertzel.c b/ch32v/ch32v203-goertzel/adcgoertzel.c index 267a65b..9c4103d 100644 --- a/ch32v/ch32v203-goertzel/adcgoertzel.c +++ b/ch32v/ch32v203-goertzel/adcgoertzel.c @@ -71,8 +71,20 @@ SOFTWARE. #define PWM_PERIOD (31-1) //For 27.0MHz, use 36MHz if quadrature -- It appears to be good for *244 in the table? WHY 26MHz???!?!!? #define ADC_BUFFSIZE 512 + +#define GOERTZEL_BUFFER 8192 + volatile uint16_t adc_buffer[ADC_BUFFSIZE]; +//const int32_t g_goertzel_omega_per_sample = 151198; // 51/128 * 3.14159 * 65536 * 2 +//const int32_t g_goertzel_coefficient = -88021;//2 * cos( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592) * 65536; +//const int32_t g_goertzel_coefficient_s = 97118;//2 * sin( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592 ) * 65536; +const int32_t g_goertzel_omega_per_sample = 1238618695; // 47/256 -> 27.01920 MHz +const int32_t g_goertzel_coefficient = 870249096; +const int32_t g_goertzel_coefficient_s = 1963250500; + + + void SetupADC() { // XXX TODO -look into PGA @@ -193,10 +205,6 @@ uint32_t tc; volatile uint16_t * adc_tail = adc_buffer; -const uint32_t g_goertzel_omega_per_sample = 41016; // 51/128 * 3.14159 * 65536 -const uint32_t g_goertzel_coefficient = 106228;//2 * cos( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592) * 65536; -const uint32_t g_goertzel_coefficient_s = 76781;//2 * sin( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592 ) * 65536; - uint32_t g_goertzel_samples; uint32_t g_goertzel_outs; int32_t g_goertzelp, g_goertzelp2; @@ -237,26 +245,40 @@ void DMA1_Channel1_IRQHandler( void ) // Here is where the magic happens. int32_t goertzel; - #define ITERATION(x) \ - t = (adc_tail[x] - 2048)<<8; \ - goertzel = t + ( ( (int64_t)(goertzel_coefficient) * goertzelp ) >> 32 ) - goertzelp2; \ - goertzelp2 = goertzelp; \ - goertzelp = goertzel; \ + #define INFADC 2 + const int ofs = (-2048) << INFADC; + + t = ((adc_tail[0])<> 32 ) - goertzelp2; + goertzelp2 = goertzelp; + goertzelp = goertzel; + + t = ((adc_tail[1])<> 32 ) - goertzelp2; + goertzelp2 = goertzelp; + goertzelp = goertzel; + + t = ((adc_tail[2])<> 32 ) - goertzelp2; + goertzelp2 = goertzelp; + goertzelp = goertzel; + + t = ((adc_tail[3])<> 32 ) - goertzelp2; + goertzelp2 = goertzelp; + goertzelp = goertzel; - ITERATION( 0 ); - ITERATION( 1 ); - ITERATION( 2 ); - ITERATION( 3 ); adc_tail+=4; goertzel_samples+=4; if( adc_tail == adc_buffer_top ) adc_tail = adc_buffer; - if( goertzel_samples == 128 ) + if( goertzel_samples == GOERTZEL_BUFFER ) { - g_goertzelp_store = goertzelp; + g_goertzelp_store = goertzelp - (g_goertzel_omega_per_sample>>(29-16)); g_goertzelp2_store = goertzelp2; - goertzelp = 0; + + goertzelp = g_goertzel_omega_per_sample>>(29-16); goertzelp2 = 0; g_goertzel_outs++; @@ -294,7 +316,17 @@ void InnerLoop() ssd1306_refresh(); ssd1306_setbuf(0); - printf( "%3d %6d %6d\n", g_goertzel_outs,g_goertzelp2_store, g_goertzelp_store ); + int32_t rr = (((int64_t)(g_goertzel_coefficient ) * (int64_t)g_goertzelp_store<<1)>>32) - (g_goertzelp2_store); \ + int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)g_goertzelp_store<<1)>>32); \ + + rr>>=4; + ri>>=4; + int s = rr * rr + ri * ri; + int x = 1<<( ( 32 - __builtin_clz(s) )/2); + x = (x + s/x)/2; + x = (x + s/x)/2; + + printf( "%6d %8d %8d - %8d %8d - %8d\n", g_goertzel_outs,g_goertzelp2_store, g_goertzelp_store, rr, ri, x ); // Delay_Ms(940); } diff --git a/ch32v/ch32v203-goertzel/ttest/demo_run.ods b/ch32v/ch32v203-goertzel/ttest/demo_run.ods index a70914f..f7cec96 100644 Binary files a/ch32v/ch32v203-goertzel/ttest/demo_run.ods and b/ch32v/ch32v203-goertzel/ttest/demo_run.ods differ diff --git a/ch32v/ch32v203-goertzel/ttest/ttest.c b/ch32v/ch32v203-goertzel/ttest/ttest.c index 30306ab..9f9f5f6 100644 --- a/ch32v/ch32v203-goertzel/ttest/ttest.c +++ b/ch32v/ch32v203-goertzel/ttest/ttest.c @@ -3,7 +3,7 @@ #include uint32_t g_goertzel_omega_per_sample; -uint32_t g_goertzel_coefficient; +int32_t g_goertzel_coefficient; int32_t g_goertzel_coefficient_s; uint32_t g_goertzel_samples; uint32_t g_goertzel_outs; @@ -12,23 +12,23 @@ int32_t g_goertzelp_store, g_goertzelp2_store; int main() { - - g_goertzel_omega_per_sample = 4.0/128 * 3.1415926535*2.0*65536; - g_goertzel_coefficient = 2 * cos( g_goertzel_omega_per_sample / 65536.0 ) * 65536; - g_goertzel_coefficient_s = 2 * sin( g_goertzel_omega_per_sample / 65536.0 ) * 65536; + //XXX XXX NOTE If you are computing the coefficients, you can plug the value in here. + g_goertzel_omega_per_sample = (47.0/256) * 3.1415926535*2.0*(1<<29); + g_goertzel_coefficient = 2 * cos( g_goertzel_omega_per_sample / (double)(1<<29) ) * (1<<30); + g_goertzel_coefficient_s = 2 * sin( g_goertzel_omega_per_sample / (double)(1<<29) ) * (1<<30); - const double AomegaPerSample = g_goertzel_omega_per_sample/65536.0; + const double AomegaPerSample = g_goertzel_omega_per_sample/(double)(1<<29); const int AnumSamples = 256; // enough to go from 0 to 2pi double Acoeff = 2 * cos( AomegaPerSample ) * 65536; double Acoeff_s = 2 * sin( AomegaPerSample ) * 65536; double Asprev = AomegaPerSample * 65536; double Asprev2 = 0; -printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertzel_coefficient, g_goertzel_coefficient_s, Acoeff, Acoeff_s, AomegaPerSample ); + printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertzel_coefficient, g_goertzel_coefficient_s, Acoeff, Acoeff_s, AomegaPerSample ); - g_goertzelp = g_goertzel_omega_per_sample; + g_goertzelp = g_goertzel_omega_per_sample>>(29-16); int32_t goertzel_coefficient = g_goertzel_coefficient; int32_t goertzelp2 = g_goertzelp2; @@ -48,11 +48,10 @@ printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertze int32_t goertzel; #define ITERATION(x) \ - t = sin( AomegaPerSample * (x+js*4) ) * 32768; \ + t = sin( AomegaPerSample * (x+js*4) ) * 16384*0; \ \ /* Fixed */ \ - printf( ">> %d + %d((%d<<8*%d<<8)>>32) - %d -", t, ( ( ((int64_t)(goertzel_coefficient)<<8) * (int64_t)(goertzelp<<8) ) >> 32 ), goertzel_coefficient, goertzelp, goertzelp2 ); \ - goertzel = t + ( ( ((int64_t)(goertzel_coefficient)<<8) * (int64_t)(goertzelp<<8) ) >> 32 ) - goertzelp2; \ + goertzel = t + ( ( (((int32_t)(goertzel_coefficient))) * ((((int64_t)goertzelp)<<2)) ) >> 32 ) - goertzelp2; \ goertzelp2 = goertzelp; \ goertzelp = goertzel; \ \ @@ -64,11 +63,11 @@ printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertze /*printf( "%d,%d,%d,%d\n", ( ( (int64_t)(goertzel_coefficient) * (int64_t)goertzelp ) >> 32 ) , goertzel_coefficient, goertzelp, goertzelp2 ); */ \ \ {\ - int32_t rr = (((int64_t)(g_goertzel_coefficient << 15) * (int64_t)goertzelp)>>32) - (goertzelp2); \ - int32_t ri = (((int64_t)(g_goertzel_coefficient_s << 15) * (int64_t)goertzelp)>>32); \ + int32_t rr = (((int64_t)(g_goertzel_coefficient ) * (int64_t)goertzelp<<1)>>32) - (goertzelp2); \ + int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)goertzelp<<1)>>32); \ /*printf( "%3d %10d %10d %10d %10d / %9d %9d %9d * ", js*4+x, rr, ri, goertzelp, goertzelp2, goertzel_coefficient, g_goertzel_omega_per_sample, t );*/ \ /*printf( "%4d %10d %10d (%10d %10d) ", js*4+x, goertzelp, goertzelp2, goertzel_coefficient, g_goertzel_coefficient_s );*/ \ - printf( "%4d %10d %10d (%10d [%d+%d*=%lld] %10d) ", js*4+x, goertzelp, goertzelp2, rr, t, g_goertzel_coefficient, ( ( (int64_t)(goertzel_coefficient<<8) * (int64_t)(goertzelp2<<8) ) >> 32 ), ri ); \ + printf( "%4d %10d %10d (%10d %10d) ", js*4+x, goertzelp, goertzelp2, rr, ri ); \ \ float Apower = Asprev*Asprev + Asprev2*Asprev2 - (Acoeff * Asprev * Asprev2); \ double ArR = 0.5 * Acoeff * Asprev / 65536 - Asprev2; \ @@ -106,5 +105,7 @@ printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertze g_goertzelp = goertzelp; g_goertzel_samples = goertzel_samples; + printf( "Constants:\nconst int32_t g_goertzel_omega_per_sample = %d;\nconst int32_t g_goertzel_coefficient = %d;\nconst int32_t g_goertzel_coefficient_s = %d;\n", g_goertzel_omega_per_sample, g_goertzel_coefficient, g_goertzel_coefficient_s ); + }