From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Tue, 1 Jul 2014 21:49:43 +0000 (+0000)
Subject: goertzal and 2m rx code
X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=9799a84dc66cf78f7d3217ac05ef33220007b150;p=freetel-svn-tracking.git

goertzal and 2m rx code

git-svn-id: https://svn.code.sf.net/p/freetel/code@1724 01035d8c-6547-0410-b346-abe4f91aad63
---

diff --git a/misc/.gitignore b/misc/.gitignore
deleted file mode 100644
index e69de29b..00000000
diff --git a/misc/goertzal/goertzal.c b/misc/goertzal/goertzal.c
new file mode 100644
index 00000000..22e4d95f
--- /dev/null
+++ b/misc/goertzal/goertzal.c
@@ -0,0 +1,312 @@
+/*
+  goertzal.c
+  David Rowe & Matthew Cook
+  21 June 2014
+
+  Step by Step port of Goertzal algorithm to fixed point, and tone
+  decoder automated testing.
+
+  ref:
+    http://en.wikipedia.org/wiki/Goertzel_algorithm
+
+  usage:
+    $ gcc goertzal.c -o goertzal -Wall -lm
+    $ ./goertzal
+*/
+
+#include <assert.h>
+#include <stdio.h>
+#include <math.h>
+
+#define FS   8000
+#define F    1000
+#define N     768
+#define AMP   512
+
+/* Vanilla float version.  Note optional printfs to dump state 
+   variables for plotting */
+
+float goertzal(short x[], int nmax, float coeff) {
+    float s, power;
+    float sprev, sprev2;
+    int   n;
+
+    sprev = 0;
+    sprev2 = 0;
+    for(n=0; n<nmax; n++) {
+        s = x[n] + coeff * sprev - sprev2;
+        sprev2 = sprev;
+        sprev = s;
+        //printf("%f\n", s);
+    }
+
+    power = sprev2*sprev2 + sprev*sprev - coeff*sprev*sprev2;
+
+    return power;
+}
+
+/* Just coeff in fixed point, lets us test if this has the right scaling. */
+
+float goertzal1(short x[], int nmax, float coeff) {
+    float s, power;
+    short coeff_fix;
+    float sprev, sprev2;
+    int   n;
+
+    coeff_fix = (1<<14)*coeff;
+
+    sprev = 0;
+    sprev2 = 0;
+    for(n=0; n<N; n++) {
+        s = x[n] + (coeff_fix * sprev)/(1<<14) - sprev2;
+        sprev2 = sprev;
+        sprev = s;       
+    }
+
+    power = sprev2*sprev2 + sprev*sprev - coeff*sprev*sprev2;
+
+    return power;
+}
+
+/* main loop in fixed point */
+
+float goertzal2(short x[], int nmax, float coeff) {
+    float power, sprev, sprev2;
+    short coeff_q14;
+    short z, zprev, zprev2;
+    int   mult;
+    int   n;
+
+    coeff_q14 = (1<<14)*coeff; /* this could be a pre-computed constant */
+
+    /*
+         s = x[n] + (coeff * sprev) - sprev2
+      s(n) = x[n] + (coeff * s[n-1]) - s[n-2]
+      
+      s has a maximum value of about 2^21, so allowing for the sign
+      bit a suitable format for 32 bit numbers would be Q22.10, that
+      gives us 21 mag bits to left of the decimal and 10 to the right.
+      
+      So lets multiply both sides of by 2^10:
+
+      2^10(s[n]) = 2^10(x[n] + (coeff * s[n-1]) - s[n-2])         
+      2^10(s[n]) = 2^10(x[n]) + 2^10((coeff * s[n-1]) - 2^10(s[n-2])
+
+      However this means we need 32 bit storage: s, sprev, and sprev2
+      would all be 32 bits which also means the (coeff * sprev) multiply
+      would be 16 by 32 bits.  Yuck.  
+
+      So lets convert from 32 bits to 16 bits. This is like stripping
+      off the least sig 16 bits, or dividing by 2^16, which is converting
+      from Q22.10 to Q6.10.  We move to C bit shift notation below as it's
+      clearer in plain text.
+
+              2^10(s[n]) = 2^10(x[n] + (coeff * s[n-1]) - s[n-2])         
+                s[n]<<10 =  (x[n] + (coeff * s[n-1]) - s[n-2])<<10         
+             (s<<10)>>16 = ((x[n] + (coeff * s[n-1]) - s[n-2])<<10)>>16
+                 s[n]>>6 =  (x[n] + (coeff * s[n-1]) - s[n-2])>>6
+                 s[n]>>6 =   x[n]>>6 + (coeff * (s[n-1]>>6)) - s[n-2]>>6
+
+      Note s>>6, s[n-1]>>6, s[n-2]>>6 must all be in the same format,
+      as they are just time delayed versions of each other.  In
+      particular the middle term is expressed as: 
+
+      (coeff * s[n-1])>>6 = (coeff * (s>[n-1]>.6)) to keep s[n-1]>>6 in
+      the same format as s[n] and s[n-2].  This is subtle, but important.
+
+      A very neat trick is to use a change of variables:
+
+      z[n] = s[n]>>6
+      
+      Now we have:
+      
+      z[n] = x[n]>>6 + (coeff * z[n-1]) - z[n-2]
+
+      Now coeff is stored in Q2.14, so:
+
+      z[n] = x[n]>>6 + ((coeff<<14) * z[n-1])>>14 - z[n-2]
+      z[n] = x[n]>>6 +  (coeff_q14 * z[n-1]) >>14 - z[n-2]
+     
+      Note we need the >>14 to balance out the scaling of coeff.  We
+      multiply two 16 bit numbers to get a 32 bit result.  This can be
+      scaled back down to 16 bits before being added to the
+      other terms.  Numbers being added must all have the same format
+      (same scaling).
+
+      Finally, we can recover s[n]:
+
+      sn = z[n]<<6
+    */
+
+    zprev = 0;
+    zprev2 = 0;
+    for(n=0; n<nmax; n++) {
+        mult = (int)coeff_q14 * (int)zprev;
+        z = (x[n]>>6) + (int)(mult>>14) - zprev2;
+        zprev2 = zprev;
+        zprev = z;       
+    }
+
+    sprev   = (float)zprev*pow(2,6);
+    sprev2  = (float)zprev2*pow(2,6);
+    
+    power = sprev2*sprev2 + sprev*sprev - coeff*sprev*sprev2;
+
+    return power;
+}
+
+/* lastly, final power calculation in fixed point */
+
+float goertzal3(short x[], int nmax, float coeff) {
+    short coeff_q14;
+    short z, zprev, zprev2;
+    int   mult, pz;
+    int   n;
+
+    coeff_q14 = (1<<14)*coeff;
+
+    zprev = 0;
+    zprev2 = 0;
+    for(n=0; n<nmax; n++) {
+        mult = (int)coeff_q14 * (int)zprev;
+        z = (x[n]>>6) + (mult>>14) - zprev2;
+        zprev2 = zprev;
+        zprev = z;       
+    }
+
+    /* 
+       Lastly, lets convert the power term to fixed point:
+
+       pz = z[n-2]*z[n-2] + z[n-1]*z[n-1] + coeff*z[n-1]*z[n-2]
+       pz = z[n-2]*z[n-2] + z[n-1]*z[n-1] + (coeff<<14)*z[n-1]*z[n-2]>>14
+
+       Given z[n] = s[n]>>6, we get:
+
+       pz      = (s[n-2]*s[n-2] + s[n-1]*s[n-1] + coeff*s[n-1]*s[n-2]) >> 12
+       pz<<12  =  s[n-2]*s[n-2] + s[n-1]*s[n-1] + coeff*s[n-1]*s[n-2]
+               =  power
+
+       i.e power = pz<<12
+   */
+    
+    mult = (int)coeff_q14*(int)zprev;
+    pz   = zprev2*zprev2 + zprev*zprev - ((short)(mult>>14))*zprev2;
+
+    return (float)pz*pow(2.0,12);
+}
+
+void howclose(float x, float y, float delta) {
+    if (fabs((x-y)/x) < delta)
+        printf("  PASS\n");
+    else
+        printf("  FAIL\n");
+}
+
+/* automated test of for goertzal tone detector */
+
+void test_tone(float coeff, float freq, float Fs, float gaindB, int expect_tone) {
+    float amp, p, e;
+    short x[N];
+    int   n, tone;
+
+    amp = AMP*pow(10.0, gaindB/20.0);
+    e   = 0.0;
+    for(n=0; n<N; n++) {
+        x[n] = amp*cos(2.0*M_PI*(freq/Fs)*n);
+        e += pow(x[n],2.0);
+    }
+
+    /* 
+       Time domain energy of sinewave e = N(AMP^2)/2
+       If all energy is in sine wave, goertzal output p = (AMP*N/2)^2
+       So we would expect p = e*(N/2) if tone is present              
+
+       So by comparing power in signal to power from goertzal filter 
+       we can make decide if tone is presnet.  The sweep results from
+       Test2/pgoertzal.m indicate the power is 10dB (linear 0.1) down
+       at the edges of the detection mask.
+    */
+
+    p = goertzal3(x, N, coeff);
+    if (p > e*(N/2)*0.1)
+        tone = 1;
+    else
+        tone = 0;
+    printf("  p: %e e: %e expect: %d  we got: %d ", p, e*N/2, expect_tone, tone);
+    if (tone == expect_tone)
+        printf(" PASS\n");
+    else
+        printf(" FAIL\n");
+}
+
+int main(void) {
+    float  w, coeff, f, Fs, fctss; 
+    float  ideal_goertzal_pwr, goertzal_float_pwr;
+    float  goertzal1_pwr, goertzal2_pwr, goertzal3_pwr;
+    FILE  *fpwr;
+    short x[N];
+    int   n;
+
+    // Test 1: single tone
+
+    w = 2.0* M_PI * ((float)F/FS);
+    coeff = 2.0* cos(w);
+
+    //printf("coeff = %f\n", coeff);
+
+    for(n=0; n<N; n++) {
+        x[n] = AMP*cos(2.0*M_PI*((float)F/FS)*n);
+    }
+
+    /* "ideal" DFT bin power should be (AMP*N/2)^2 */
+    
+    ideal_goertzal_pwr = pow((float)AMP*N/2,2.0);
+    goertzal_float_pwr = goertzal(x, N, coeff);
+    goertzal1_pwr      = goertzal1(x, N, coeff);
+    goertzal2_pwr      = goertzal2(x, N, coeff);
+    goertzal3_pwr      = goertzal3(x, N, coeff);
+ 
+    printf("Test 1:\n");
+    printf("  ideal.: %e\n", pow((float)AMP*N/2,2.0));
+    printf("  float.: %e ", goertzal1_pwr);
+    howclose(ideal_goertzal_pwr, goertzal_float_pwr, 0.02);
+    printf("  step 1: %e ", goertzal1_pwr);
+    howclose(ideal_goertzal_pwr, goertzal1_pwr, 0.02);
+    printf("  step 2: %e ", goertzal2_pwr);
+    howclose(ideal_goertzal_pwr, goertzal2_pwr, 0.02);
+    printf("  step 3: %e ", goertzal3_pwr);
+    howclose(ideal_goertzal_pwr, goertzal3_pwr, 0.02);
+        
+    /* Test 2: sweep and determine 3dB points for simulated CTCSS
+       tone, examine with pgoertzal.m */
+   
+    Fs = 1000.0;
+
+    fctss = 91.5;
+    w = 2.0* M_PI * (fctss/Fs);
+    coeff = 2.0* cos(w); 
+    fpwr = fopen("pwr.txt", "wt");
+    assert(fpwr != NULL);
+
+    for(f=0; f<250; f+=0.1) {
+        for(n=0; n<N; n++) {
+            x[n] = AMP*cos(2.0*M_PI*(f/Fs)*n);
+        }
+        fprintf(fpwr,"%e %e %e\n", f, goertzal(x, N, coeff), goertzal3(x, N, coeff));
+    }
+    fclose(fpwr);
+
+    /* Test 3: Check response of tone detector at different tone amplitudes and 
+       frequencies */
+
+    printf("\nTest 2:\n");
+    test_tone(coeff, 91.5,  Fs,   0.0, 1);
+    test_tone(coeff, 91.5,  Fs,  -6.0, 1);
+    test_tone(coeff, 91.5,  Fs, -12.0, 1);
+    test_tone(coeff, 90.59, Fs,   0.0, 1);
+    test_tone(coeff, 92.42, Fs,   0.0, 1);
+    test_tone(coeff, 88.0 , Fs,   0.0, 0);
+    test_tone(coeff, 95.0 , Fs,   0.0, 0);
+   
+    return 0;
+}
diff --git a/misc/goertzal/pgoertzal.m b/misc/goertzal/pgoertzal.m
new file mode 100644
index 00000000..599676ea
--- /dev/null
+++ b/misc/goertzal/pgoertzal.m
@@ -0,0 +1,32 @@
+% pgoertzal.m
+% David Rowe
+% 21 June 2014
+%
+% Helper Octave script to plot Goertzal test data
+
+load pwr.txt; 
+max_val = max(pwr(:,2));
+max_val_dB = 10*log10(max_val);
+
+figure(1)
+
+subplot(211)
+plot(pwr(:,1),10*log10(pwr(:,2)),'b;float;'); 
+hold on;
+plot(pwr(:,1),10*log10(pwr(:,3)),'g;goertzal3;'); 
+hold off;
+axis([0 250 max_val_dB-60  max_val_dB])
+xlabel("Freq (Hz)");
+grid
+
+subplot(212)
+st = 85/0.1;
+en = 95/0.1;
+plot(pwr(st:en,1),10*log10(pwr(st:en,2)),'b;float;'); 
+hold on;
+plot(pwr(st:en,1),10*log10(pwr(st:en,3)),'g;goertzal3;'); 
+plot([89 90.59 90.59 92.42 92.42 94],[0 0 (max_val_dB-3) (max_val_dB-3) 0 0],'r;mask;')
+hold off
+axis([89 94 max_val_dB-12  max_val_dB])
+xlabel("Freq (Hz)");
+grid
diff --git a/misc/octave/twometre_rx.m b/misc/octave/twometre_rx.m
new file mode 100644
index 00000000..97e7d320
--- /dev/null
+++ b/misc/octave/twometre_rx.m
@@ -0,0 +1,196 @@
+% twometre_rx.m
+% David Rowe
+% 28 June 2014
+%
+% Working and simulations for 2m SDR rx
+
+% Input tuned cct and matching from Fig 2-18(D) Borwick ----------------------------
+%
+%        /----+----\
+%        |    |    |
+%        C2   |    |
+%        |    |    |
+%  /-Rs--+    L    Rl
+%  |     |    |    |
+% (~)    C1   |    |
+%  |     |    |    |
+%  \-----+----+----/
+%
+% Purpose of input tuned cct is:
+%   + Zmatch 50 ohms to 1500 ohms
+%   + some sort of BPF
+%   + have realisable component, e.g. L not too small
+
+input_bpf = 0;
+
+if input_bpf
+  C1=68E-12;
+  C2=15E-12;
+  L=100E-9;
+  Rs=50;
+
+  Rl = Rs*(1+C1/C2)^2;
+  C=1/(1/C1+1/C2);
+
+  f=1/(2*pi*sqrt(L*C));
+  Xl = 2*pi*f*L;
+  Q = Rl/Xl;
+  BW=f/Q;
+
+  printf("C1 %2.0f pF C2 %2.0f pF L %3.0f nH Rs %2.0f Rl %4.0f f %5.2f MHz BW %3.2f MHz\n", ...
+       C1*1E12, C2*1E12, L*1E9, Rs, Rl, f/1E6, BW/1E6);
+end
+
+% FM simulation ---------------------------------------------------------
+%
+% RF signal is mixed down to 0 to 500 kHz
+% Analog filter 20dB down at 500 kHz
+% Sample real signal Fs=1MS/s with single ADC
+% test with interferer 25kHz away and 20dB higher
+% complex mix down to baseband
+% decimate down to Fs2=25kHz, (+/- 12.5kHz, a single 25kHz FM channel)
+% FM demod
+%
+% TODO
+%   [X] set deviation
+%   [X] hard limiting
+%   [X] FM demod
+%   [ ] noise
+%   [ ] attenuation
+%   [ ] model system gain, and effect of ADC
+%   [ ] model interferer, determine how many ADC bits we need
+%   [ ] LPF
+
+% regular natural binary quantiser
+
+function index = quantise_value(value, min_value, max_value, num_levels)
+  norm = (value - min_value)/(max_value - min_value);
+  index = floor(num_levels*norm + 0.5);
+  index(find(index < 0)) = 0;
+  index(find(index > (num_levels-1))) = num_levels-1;
+endfunction
+
+function value = unquantise_value(index, min_value, max_value, num_levels)
+  step = (max_value - min_value)/num_levels;
+  value = min_value + step*(index);
+endfunction 
+
+more off;
+
+% constants -------------------------
+
+Fs = 1E6;
+fc = 250E3; wc = 2*pi*fc/Fs;
+fm = 1E3; wm = 2*pi*fm/Fs;
+
+f_max_deviation = 5E3; w_max_deviation = 2*pi*f_max_deviation/Fs;
+t = 1:1E6;
+
+fi = fc+50E3; wi = 2*pi*fi/Fs;
+Ai = 10;
+
+Fs2 = 50E3;
+method = 1;
+M=Fs/Fs2;
+[b,a] = cheby1(8, 1, 20E3/Fs);
+
+clip = 10;
+levels = 2^10;
+
+% simulation -------------------------
+
+% signal at input to ADC
+
+mod = cos(wm*t);
+wt = wc*t + w_max_deviation*mod;
+tx = cos(wt) + j*sin(wt) + Ai*(cos(wi*t) + j*sin(wi*t));
+%tx = real(tx);
+
+% simulate ADC (sign bit only)
+
+rx = real(tx);
+indexes = quantise_value(rx, -clip, clip, 1024);
+rx = unquantise_value(indexes, -clip, clip, 1024);
+%rx = sign(real(tx)) + j*sign(imag(tx));
+
+% downconvert to complex baseband and filter/decimate to Fs2
+
+rx_bb   = rx .* exp(-j*wc*t);
+rx_filt = filter(b, a, rx_bb);
+rx_bb2  = rx_filt(1:M:length(rx_filt));
+rx_bb2 = sign(real(rx_bb2)) + j*sign(imag(rx_bb2));
+
+% demodulate 
+
+if method == 1
+
+  % atan first, then differentiate.  However we need to unwrap
+  % phase to avoid 0 -> 2*pi jumps which all gets a bit messy
+
+  phase = unwrap(atan2(imag(rx_bb2),real(rx_bb2)));
+  demod = M*2*pi*diff(phase);
+end
+
+% differentiate phase first in rect coords which avoids trig functions
+% and nasty 0 -> 2*pi wrap around and allows approximation to avoid
+% trig functions altogether.  Much smarter.
+
+if method == 2
+
+  % multiplying by complex conjugate subtracts phases
+  %   exp(ja)*exp(jb)' = exp(ja)*exp(jb)'
+  %                    = exp(ja)*exp(-jb)
+  %                    = exp(j(b-a))
+
+  l = length(rx_bb2);
+  rx_bb_diff = rx_bb2(2:l) .* conj(rx_bb2(1:l-1));
+
+  % for x small, tan(x) = sin(x) = x = imag(exp(jx))
+
+  demod = 2*pi*M*imag(rx_bb_diff);
+
+end
+
+% 300-3000Hz BPF on demodualted audio
+
+[b_bpf a_bpf] = cheby1(4, 2, [300/Fs2 3000/Fs2]);
+audio_out = filter(b_bpf, a_bpf, demod);
+
+% plot results --------------------------------------
+
+figure(1)
+clf
+subplot(311)
+plot(real(tx(1:100)))
+ylabel('ADC input')
+subplot(312)
+plot(real(rx(1:100)))
+ylabel('ADC output')
+subplot(313)
+plot(audio_out(1:400))
+ylabel('Audio Out')
+%axis([1 400 -1 1])
+
+figure(2)
+subplot(311)
+[p,f] = pwelch(tx);
+plot(f,10*log10(p));
+ylabel('ADC Input');
+subplot(312)
+[p,f] = pwelch(rx);
+plot(f,10*log10(p));
+ylabel('ADC Output')
+subplot(313)
+[p,f] = pwelch(audio_out);
+plot(f,10*log10(p));
+ylabel('Input to Demod');
+
+if 0
+figure(3)
+clf;
+H=freqz(b,a,Fs/10);
+HdB=20*log10(abs(H));
+plot((1:Fs2/10)*10/1000, HdB(1:Fs2/10));
+axis([1 Fs2/1000 -40 0]);
+grid
+end