daniel/lolra
1
0
Fork 0
mirror of https://github.com/cnlohr/lolra.git synced 2026-06-15 07:19:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Goertzels is working

Browse Source
This commit is contained in:
cnlohr
2024-06-23 23:32:35 -07:00
parent 7052e8c111
commit 64335ae653
3 changed files with 64 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch32v/ch32v203-goertzel/adcgoertzel.c
+49 -17
View File
@@ -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])<<INFADC)+ofs;
goertzel = t + ( ( (((int32_t)(goertzel_coefficient))) * ((((int64_t)goertzelp)<<2)) ) >> 32 ) - goertzelp2;
goertzelp2 = goertzelp;
goertzelp = goertzel;
t = ((adc_tail[1])<<INFADC)+ofs;
goertzel = t + ( ( (((int32_t)(goertzel_coefficient))) * ((((int64_t)goertzelp)<<2)) ) >> 32 ) - goertzelp2;
goertzelp2 = goertzelp;
goertzelp = goertzel;
t = ((adc_tail[2])<<INFADC)+ofs;
goertzel = t + ( ( (((int32_t)(goertzel_coefficient))) * ((((int64_t)goertzelp)<<2)) ) >> 32 ) - goertzelp2;
goertzelp2 = goertzelp;
goertzelp = goertzel;
t = ((adc_tail[3])<<INFADC)+ofs;
goertzel = t + ( ( (((int32_t)(goertzel_coefficient))) * ((((int64_t)goertzelp)<<2)) ) >> 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);
}
ch32v/ch32v203-goertzel/ttest/demo_run.ods
BIN
View File
Binary file not shown.
ch32v/ch32v203-goertzel/ttest/ttest.c
+15 -14
View File
@@ -3,7 +3,7 @@
#include <stdint.h>
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 );
}