From b2995f435f37181170ed77bae3c056fd45d7a4fe Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 6 Oct 2024 23:06:59 -0700 Subject: [PATCH] Working with phasor re-rotation --- ch32v/ch32v203-goertzel/adcgoertzel.c | 43 +++++++++++++++++- ch32v/ch32v203-goertzel/ttest/Makefile | 8 +++- ch32v/ch32v203-goertzel/ttest/floattestt.c | 44 +++++++++++++++++++ .../ttest/phasor_rotation_test.c | 43 ++++++++++++++++++ ch32v/ch32v203tx/ch32v203tx.c | 4 +- ch32v/lib/calculator.html | 29 +++++++++--- 6 files changed, 161 insertions(+), 10 deletions(-) create mode 100644 ch32v/ch32v203-goertzel/ttest/floattestt.c create mode 100644 ch32v/ch32v203-goertzel/ttest/phasor_rotation_test.c diff --git a/ch32v/ch32v203-goertzel/adcgoertzel.c b/ch32v/ch32v203-goertzel/adcgoertzel.c index 6f252a1..be2a2d5 100644 --- a/ch32v/ch32v203-goertzel/adcgoertzel.c +++ b/ch32v/ch32v203-goertzel/adcgoertzel.c @@ -81,6 +81,12 @@ volatile uint16_t adc_buffer[ADC_BUFFSIZE]; int g_volume_pwm = 127; // 0 - 127 (100%) (but you can go over 100) +int32_t g_goertzel_phasor_r = 32768; +int32_t g_goertzel_phasor_i = 0; + +int32_t g_goertzel_advance_r = 32768; +int32_t g_goertzel_advance_i = 0; + #if 0 int g_pwm_period = (30-1); int g_goertzel_buffer = (752); @@ -390,6 +396,36 @@ void DMA1_Channel1_IRQHandler( void ) int32_t rr = (((int64_t)(g_goertzel_coefficient ) * (int64_t)zp<<1)>>32) - (zp2); int32_t ri = (((int64_t)(g_goertzel_coefficient_s) * (int64_t)zp<<1)>>32); + // Advanced the current goertzel advance + // phasor = phasor * advance; + // real = real * real - imag * imag; + // imag = real * imag + real * imag; + // Sometimes you would bias the output here so that when truncating down you don't perpetually decay. + // But experimentally, it didn't make a difference. + int32_t temp = (g_goertzel_phasor_r * g_goertzel_advance_i + g_goertzel_phasor_i * g_goertzel_advance_r) >> 15; + g_goertzel_phasor_r = (g_goertzel_phasor_r * g_goertzel_advance_r - g_goertzel_phasor_i * g_goertzel_advance_i) >> 15; + g_goertzel_phasor_i = temp; + + // Fixup phasor over time to prevent it from dacaying. + #define ABS(x) (((x)<0)?-(x):(x)) + int s_phasor = g_goertzel_phasor_r * g_goertzel_phasor_r + g_goertzel_phasor_i * g_goertzel_phasor_i; + int intensity_phasor = (ABS(g_goertzel_phasor_r) + ABS(g_goertzel_phasor_i)) * 26100 / 32768 + 1; // Found experimentally (Also try to avoid divide-by-zero. + intensity_phasor = (intensity_phasor + s_phasor/intensity_phasor)/2; + intensity_phasor = (intensity_phasor + s_phasor/intensity_phasor)/2; + if( intensity_phasor < 32760 ) + { + // It is decaying, this is equivelent to f = f * 1.000244141 + g_goertzel_phasor_r += g_goertzel_phasor_r >> 12; + g_goertzel_phasor_i += g_goertzel_phasor_i >> 12; + } + + // Now, rotate rr, ri by that phasor. + temp = (g_goertzel_phasor_r * ri + g_goertzel_phasor_i * rr) >> 15; + rr = (g_goertzel_phasor_r * rr - g_goertzel_phasor_i * ri) >> 15; + ri = temp; + + // rr, ri are now in the correct frame of reference. Continue computing. + qibaselogs[qibaselogs_head] = ((uint16_t)rr) | (((uint16_t)ri)<<16); qibaselogs_head = ( qibaselogs_head + 1 ) & ((LOG_GOERTZEL_LIST)-1); @@ -400,8 +436,9 @@ void DMA1_Channel1_IRQHandler( void ) //int intensity = 1<<( ( 32 - __builtin_clz(s) )/2); #define ABS(x) (((x)<0)?-(x):(x)) int intensity = (ABS(rr) + ABS(ri)) * 26100 / 32768; // Found experimentally (Also try to avoid divide-by-zero. - if( intensity == 0 ) - intensity = 1; + //if( intensity == 0 ) + // intensity = 1; + intensity++; intensity = (intensity + s/intensity)/2; intensity = (intensity + s/intensity)/2; intensity_average = intensity_average - (intensity_average>>12) + (intensity>>6); @@ -714,6 +751,8 @@ void HandleHidUserReportOutComplete( struct _USBState * ctx ) if( numconfigs > 3) g_goertzel_coefficient = configs[5]; if( numconfigs > 4) g_goertzel_coefficient_s = configs[6]; if( numconfigs > 5) g_exactcompute = configs[7]; + if( numconfigs > 6) g_goertzel_advance_r = configs[8]; + if( numconfigs > 7) g_goertzel_advance_i = configs[9]; // Need to reset so we don't blast by. g_goertzel_samples = 0; diff --git a/ch32v/ch32v203-goertzel/ttest/Makefile b/ch32v/ch32v203-goertzel/ttest/Makefile index c835885..feb3730 100644 --- a/ch32v/ch32v203-goertzel/ttest/Makefile +++ b/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 diff --git a/ch32v/ch32v203-goertzel/ttest/floattestt.c b/ch32v/ch32v203-goertzel/ttest/floattestt.c new file mode 100644 index 0000000..191750e --- /dev/null +++ b/ch32v/ch32v203-goertzel/ttest/floattestt.c @@ -0,0 +1,44 @@ +#include +#include + +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); + + } + + } +} diff --git a/ch32v/ch32v203-goertzel/ttest/phasor_rotation_test.c b/ch32v/ch32v203-goertzel/ttest/phasor_rotation_test.c new file mode 100644 index 0000000..bbdf615 --- /dev/null +++ b/ch32v/ch32v203-goertzel/ttest/phasor_rotation_test.c @@ -0,0 +1,43 @@ +#include +#include +#include + +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; + } + +} diff --git a/ch32v/ch32v203tx/ch32v203tx.c b/ch32v/ch32v203tx/ch32v203tx.c index b35ec91..bb8bb2e 100644 --- a/ch32v/ch32v203tx/ch32v203tx.c +++ b/ch32v/ch32v203tx/ch32v203tx.c @@ -84,10 +84,10 @@ int main() TIM2->CH1CVR = 2; TIM2->CCER = TIM_CC1E | TIM_CC1P; funDigitalWrite( LEDPIN, 1 ); - Delay_Us( 2000 ); + Delay_Us( 20000 ); TIM2->CCER = TIM_CC1E; TIM2->CH1CVR = 2; funDigitalWrite( LEDPIN, 0 ); - Delay_Us( 2000 ); + Delay_Us( 20000 ); } } diff --git a/ch32v/lib/calculator.html b/ch32v/lib/calculator.html index 956c5dc..a95292a 100644 --- a/ch32v/lib/calculator.html +++ b/ch32v/lib/calculator.html @@ -24,12 +24,20 @@ function DrawSpan( rowspan, colspan, freq, target, docolor, extrastr = "" ) function Goertz( n, mhz, fr, brf, exact_compute ) { - let omega = fr * 3.1415926535*2.0; + let tau = 3.1415926535*2.0; + let omega = fr * tau; var textarea = document.getElementById("goertzeloutput"); - var g_goertzel_omega_per_sample = Math.round( ( omega*2*(1<<29)) ); + var goertzel_omega_per_sample_real = ( omega*2*(1<<29)); + var g_goertzel_omega_per_sample = Math.round( goertzel_omega_per_sample_real ); var g_goertzel_coefficient = Math.round( (2 * Math.cos( omega ) * (1<<30)) ); var g_goertzel_coefficient_s = Math.round( (2 * Math.sin( omega ) * (1<<30)) ); + + var omega_per_group = omega * brf; + var goertzel_phasor_advance_radians_per_sample = tau * (Math.round( omega_per_group / tau ) - omega_per_group / tau); + var g_goertzel_advance_r = Math.cos( goertzel_phasor_advance_radians_per_sample ) * 32768; + var g_goertzel_advance_i = Math.sin( goertzel_phasor_advance_radians_per_sample ) * 32768; + var g_exactcompute = exact_compute; textarea.value = "int g_pwm_period = ("+n+"-1);\n" + @@ -37,7 +45,9 @@ function Goertz( n, mhz, fr, brf, exact_compute ) "int g_goertzel_buffer = ("+brf+");\n" + "int32_t g_goertzel_omega_per_sample = " + g_goertzel_coefficient.toFixed(0) + "; // " + ( omega / (3.1415926535*2.0)).toFixed(6) + " of whole per step / " + mhz.toFixed(6) + "MHz\n" + "int32_t g_goertzel_coefficient = " + g_goertzel_coefficient.toFixed(0) + ";\n" + - "int32_t g_goertzel_coefficient_s = " + g_goertzel_coefficient_s.toFixed(0) + ";\n"; + "int32_t g_goertzel_coefficient_s = " + g_goertzel_coefficient_s.toFixed(0) + ";\n" + + "int32_t g_goertzel_advance_r = " + g_goertzel_advance_r.toFixed(0) + ";\n" + + "int32_t g_goertzel_advance_i = " + g_goertzel_advance_i.toFixed(0) + ";\n"; // Highlight its content textarea.select(); @@ -45,7 +55,16 @@ function Goertz( n, mhz, fr, brf, exact_compute ) // Copy the highlighted text document.execCommand("copy"); - updateWebHidDeviceWithParameters( [ (n-1)|0, brf|0, g_goertzel_omega_per_sample|0, g_goertzel_coefficient|0, g_goertzel_coefficient_s|0, exact_compute|0 ] ); + updateWebHidDeviceWithParameters( [ + (n-1)|0, + brf|0, + g_goertzel_omega_per_sample|0, + g_goertzel_coefficient|0, + g_goertzel_coefficient_s|0, + exact_compute|0, + g_goertzel_advance_r|0, + g_goertzel_advance_i|0, + ] ); } function computeTable() @@ -88,7 +107,7 @@ function computeTable() "" + "" + "" + - "
Goertzel
Goertzel (Inverse)
" + + "" + "

Click on a ordinal offset to create the C code needed for that tuning parameter. Clicking will copy-to-clipboard.

" + "

N Divisor #30 (row 3) is usually pretty good. And, try to select things near 0.25 / 0.75, and avoid 0.0, 0.5, and 1.0.

" + "

Goertzel's mode is for the ch32v203

";