From 690464aec918f8586366af9a56d3d5a8cca59323 Mon Sep 17 00:00:00 2001 From: Zilog80 Date: Thu, 20 Aug 2020 21:44:53 +0200 Subject: [PATCH] IF-IQ server/client: test -> master --- demod/iq_svcl/README.md | 47 +++ demod/iq_svcl/iq_base.c | 669 ++++++++++++++++++++++++++++++ demod/iq_svcl/iq_base.h | 123 ++++++ demod/iq_svcl/iq_client.c | 195 +++++++++ demod/iq_svcl/iq_fm.c | 464 +++++++++++++++++++++ demod/iq_svcl/iq_server.c | 838 ++++++++++++++++++++++++++++++++++++++ demod/iq_svcl/iq_svcl.h | 22 + demod/iq_svcl/plot_fft.py | 40 ++ demod/iq_svcl/scan_fft.c | 198 +++++++++ 9 files changed, 2596 insertions(+) create mode 100644 demod/iq_svcl/README.md create mode 100644 demod/iq_svcl/iq_base.c create mode 100644 demod/iq_svcl/iq_base.h create mode 100644 demod/iq_svcl/iq_client.c create mode 100644 demod/iq_svcl/iq_fm.c create mode 100644 demod/iq_svcl/iq_server.c create mode 100644 demod/iq_svcl/iq_svcl.h create mode 100644 demod/iq_svcl/plot_fft.py create mode 100644 demod/iq_svcl/scan_fft.c diff --git a/demod/iq_svcl/README.md b/demod/iq_svcl/README.md new file mode 100644 index 0000000..7867894 --- /dev/null +++ b/demod/iq_svcl/README.md @@ -0,0 +1,47 @@ + +## IQ server/client + +receive IF stream from baseband IQ via TCP, default `PORT=1280 (iq_svcl.h)`
+ + +#### Compile + `gcc -Ofast -c iq_base.c`
+ `gcc -O2 iq_server.c iq_base.o -lm -pthread -o iq_server`
+ `gcc -O2 iq_client.c -o iq_client`
+ +#### Usage/Examples + - `T1$ ./iq_server [--port ] `
+ `T2$ ./iq_client [--ip ] [--port ] --freq ` + + - Ex.1
+ [terminal 1]
+ `T1$ ./iq_server --bo 32 `    + (or   `$ ./iq_server --bo 32 - `)
+ [terminal 2]
+ `T2$ ./iq_client --freq | ./rs41mod -vx --IQ 0.0 --lp - `
+ where
+      `-0.5 < fq < 0.5`: (relative) frequency, `fq=frequency/sr`
+      ``: IF sample rate
+      `=8,16,32`: output/IF bits per (real) sample (u8, s16 or f32)
+ down-converts up to `MAX_FQ=(4+1) (iq_base.h)` channels/signals. More signals than number of CPUs/cores is not recommended.
+ (Note: If the baseband sample rate has no appropriate factors (e.g. if prime), the IF sample rate might be high and IF-processing slow.)
+ One channel can be used for scanning, `--fft ` makes FFT (2 seconds average). + The FFT is saved in `` as `;`, approx. 200 Hz per bin.
+ If no output bps is chosen (`--bo [8,16,32]`), the IF bps is equal to the baseband bps. It is recommended to use + `--bo 32` (i.e. float32) output, then no quantization noise is introduced when converting from internal float32 samples.
+ + - Ex.2
+ [terminal 1]
+ `T1$ rtl_sdr -f 403.0M -s 1920000 - | ./iq_server --fft fft_server.txt --bo 32 - 1920000 8`
+ [terminal 2]
+ `T2$ ./iq_client --freq -0.3125 | ./m10mod -c -vv --IQ 0.0 - 48000 32`
+ [terminal 3]
+ `T3$ ./iq_client --freq 0.0 | ./rs41mod -vx --IQ 0.0 - 48000 32`
+ [terminal 4]
+ `T4$ ./iq_client -1`    (*close channel 1*)
+ `T4$ ./iq_client --stop`    (*close all clients and stop server*)
+ + The `iq_server` `--fft` option immediately starts reading the IQ stream (so buffering is reduced).
+ `./iq_client --fft ` can also request FFT.
+ The IF sample rate `if_sr` is at least 48000 such that the baseband sample rate `sr` is a multiple of `if_sr`. + diff --git a/demod/iq_svcl/iq_base.c b/demod/iq_svcl/iq_base.c new file mode 100644 index 0000000..f3380cf --- /dev/null +++ b/demod/iq_svcl/iq_base.c @@ -0,0 +1,669 @@ + +/* + * compile: + * gcc -c iq_base.c + * speedup: + * gcc -O2 -c iq_base.c + * or + * gcc -Ofast -c iq_base.c + * + * author: zilog80 + */ + +/* ------------------------------------------------------------------------------------ */ + +#include +#include +#include + +#include "iq_base.h" + +/* ------------------------------------------------------------------------------------ */ + + +//static +void raw_dft(dft_t *dft, float complex *Z) { + int s, l, l2, i, j, k; + float complex w1, w2, T; + float complex _1 = (float complex)1.0; + + j = 1; + for (i = 1; i < dft->N; i++) { + if (i < j) { + T = Z[j-1]; + Z[j-1] = Z[i-1]; + Z[i-1] = T; + } + k = dft->N/2; + while (k < j) { + j = j - k; + k = k/2; + } + j = j + k; + } + + for (s = 0; s < dft->LOG2N; s++) { + l2 = 1 << s; + l = l2 << 1; + w1 = _1; + w2 = dft->ew[s]; // cexp(-I*M_PI/(float)l2) + for (j = 1; j <= l2; j++) { + for (i = j; i <= dft->N; i += l) { + k = i + l2; + T = Z[k-1] * w1; + Z[k-1] = Z[i-1] - T; + Z[i-1] = Z[i-1] + T; + } + w1 = w1 * w2; + } + } +} + +static void cdft(dft_t *dft, float complex *z, float complex *Z) { + int i; + for (i = 0; i < dft->N; i++) Z[i] = z[i]; + raw_dft(dft, Z); +} + +static void rdft(dft_t *dft, float *x, float complex *Z) { + int i; + for (i = 0; i < dft->N; i++) Z[i] = (float complex)x[i]; + raw_dft(dft, Z); +} + +static void Nidft(dft_t *dft, float complex *Z, float complex *z) { + int i; + for (i = 0; i < dft->N; i++) z[i] = conj(Z[i]); + raw_dft(dft, z); + // idft(): + // for (i = 0; i < dft->N; i++) z[i] = conj(z[i])/(float)dft->N; // hier: z reell +} + +static float bin2freq0(dft_t *dft, int k) { + float fq = dft->sr * k / /*(float)*/dft->N; + if (fq >= dft->sr/2.0) fq -= dft->sr; + return fq; +} +//static +float bin2freq(dft_t *dft, int k) { + float fq = k / (float)dft->N; + if ( fq >= 0.5) fq -= 1.0; + return fq*dft->sr; +} +//static +float bin2fq(dft_t *dft, int k) { + float fq = k / (float)dft->N; + if ( fq >= 0.5) fq -= 1.0; + return fq; +} + +static int max_bin(dft_t *dft, float complex *Z) { + int k, kmax; + double max; + + max = 0; kmax = 0; + for (k = 0; k < dft->N; k++) { + if (cabs(Z[k]) > max) { + max = cabs(Z[k]); + kmax = k; + } + } + + return kmax; +} + +static int dft_window(dft_t *dft, int w) { + int n; + + if (w < 0 || w > 3) return -1; + + for (n = 0; n < dft->N2; n++) { + switch (w) + { + case 0: // (boxcar) + dft->win[n] = 1.0; + break; + case 1: // Hann + dft->win[n] = 0.5 * ( 1.0 - cos(2*M_PI*n/(float)(dft->N2-1)) ); + break ; + case 2: // Hamming + dft->win[n] = 25/46.0 + (1.0 - 25/46.0)*cos(2*M_PI*n / (float)(dft->N2-1)); + break ; + case 3: // Blackmann + dft->win[n] = 7938/18608.0 + - 9240/18608.0*cos(2*M_PI*n / (float)(dft->N2-1)) + + 1430/18608.0*cos(4*M_PI*n / (float)(dft->N2-1)); + break ; + } + } + while (n < dft->N) dft->win[n++] = 0.0; + + return 0; +} + + +//static double ilog102 = 0.434294482/2.0; // log(10)/2 +void db_power(dft_t *dft, float complex Z[], float db[]) { // iq-samples/V [-1..1] + int i; // dBw = 2*dBv, P=c*U*U + for (i = 0; i < dft->N; i++) { // dBw = 2*10*log10(V/V0) + db[i] = 20.0 * log10(cabs(Z[i])/dft->N2+1e-20); // 20log10(Z/N) + } +} + +/* ------------------------------------------------------------------------------------ */ + +static int findstr(char *buff, char *str, int pos) { + int i; + for (i = 0; i < 4; i++) { + if (buff[(pos+i)%4] != str[i]) break; + } + return i; +} + +float read_wav_header(pcm_t *pcm) { + FILE *fp = pcm->fp; + char txt[4+1] = "\0\0\0\0"; + unsigned char dat[4]; + int byte, p=0; + int sample_rate = 0, bits_sample = 0, channels = 0; + + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "RIFF", 4)) return -1; + if (fread(txt, 1, 4, fp) < 4) return -1; + // pos_WAVE = 8L + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "WAVE", 4)) return -1; + // pos_fmt = 12L + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "fmt ", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + + if (fread(dat, 1, 2, fp) < 2) return -1; + channels = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 4, fp) < 4) return -1; + memcpy(&sample_rate, dat, 4); //sample_rate = dat[0]|(dat[1]<<8)|(dat[2]<<16)|(dat[3]<<24); + + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + //byte = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 2, fp) < 2) return -1; + bits_sample = dat[0] + (dat[1] << 8); + + // pos_dat = 36L + info + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "data", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + + + fprintf(stderr, "sample_rate: %d\n", sample_rate); + fprintf(stderr, "bits : %d\n", bits_sample); + fprintf(stderr, "channels : %d\n", channels); + + + if (bits_sample != 8 && bits_sample != 16 && bits_sample != 32) return -1; + + if (sample_rate == 900001) sample_rate -= 1; + + pcm->sr = sample_rate; + pcm->bps = bits_sample; + pcm->nch = channels; + + return 0; +} + +typedef struct { + double sumIQx; + double sumIQy; + float avgIQx; + float avgIQy; + ui32_t cnt; + ui32_t maxcnt; + ui32_t maxlim; +} iq_dc_t; +static iq_dc_t IQdc; + +int iq_dc_init(pcm_t *pcm) { + memset(&IQdc, 0, sizeof(IQdc)); + IQdc.maxlim = pcm->sr; + IQdc.maxcnt = IQdc.maxlim/32; // 32,16,8,4,2,1 + if (pcm->decM > 1) { + IQdc.maxlim *= pcm->decM; + IQdc.maxcnt *= pcm->decM; + } + + return 0; +} + +static int f32read_csample(dsp_t *dsp, float complex *z) { + + float x, y; + + if (dsp->bps == 32) { //float32 + float f[2]; + if (fread( f, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = f[0]; + y = f[1]; + } + else if (dsp->bps == 16) { //int16 + short b[2]; + if (fread( b, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = b[0]/32768.0; + y = b[1]/32768.0; + } + else { // dsp->bps == 8 //uint8 + ui8_t u[2]; + if (fread( u, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = (u[0]-128)/128.0; + y = (u[1]-128)/128.0; + } + + *z = (x - IQdc.avgIQx) + I*(y - IQdc.avgIQy); + + IQdc.sumIQx += x; + IQdc.sumIQy += y; + IQdc.cnt += 1; + if (IQdc.cnt == IQdc.maxcnt) { + IQdc.avgIQx = IQdc.sumIQx/(float)IQdc.maxcnt; + IQdc.avgIQy = IQdc.sumIQy/(float)IQdc.maxcnt; + IQdc.sumIQx = 0; IQdc.sumIQy = 0; IQdc.cnt = 0; + if (IQdc.maxcnt < IQdc.maxlim) IQdc.maxcnt *= 2; + } + + return 0; +} + + +volatile int bufeof = 0; // threads exit +volatile int rbf1; +static volatile int rbf; + + +#ifdef CLK +#include +static struct timespec t_1; +static unsigned int in_smp; +static int t_init = 0; +static double t_acc = 0; +#endif + +static int f32_cblk(dsp_t *dsp) { + + int n; + int BL = dsp->decM * blk_sz; + int len = BL; + float x, y; + ui8_t s[4*2*BL]; //uin8,int16,flot32 + ui8_t *u = (ui8_t*)s; + short *b = (short*)s; + float *f = (float*)s; + + #ifdef CLK + if ( t_init == 0 ) { + t_init = 1; + clock_gettime(CLOCK_REALTIME, &t_1); + } + #endif + + + len = fread( s, dsp->bps/8, 2*BL, dsp->fp) / 2; + + //for (n = 0; n < len; n++) dsp->thd->blk[n] = (u[2*n]-128)/128.0 + I*(u[2*n+1]-128)/128.0; + // u8: 0..255, 128 -> 0V + for (n = 0; n < len; n++) { + if (dsp->bps == 8) { //uint8 + x = (u[2*n ]-128)/128.0; + y = (u[2*n+1]-128)/128.0; + } + else if (dsp->bps == 16) { //int16 + x = b[2*n ]/32768.0; + y = b[2*n+1]/32768.0; + } + else { // dsp->bps == 32 //float32 + x = f[2*n]; + y = f[2*n+1]; + } + + dsp->thd->blk[n] = (x-IQdc.avgIQx) + I*(y-IQdc.avgIQy); + + IQdc.sumIQx += x; + IQdc.sumIQy += y; + IQdc.cnt += 1; + if (IQdc.cnt == IQdc.maxcnt) { + IQdc.avgIQx = IQdc.sumIQx/(float)IQdc.maxcnt; + IQdc.avgIQy = IQdc.sumIQy/(float)IQdc.maxcnt; + IQdc.sumIQx = 0; IQdc.sumIQy = 0; IQdc.cnt = 0; + if (IQdc.maxcnt < IQdc.maxlim) IQdc.maxcnt *= 2; + } + } + if (len < BL) bufeof = 1; + + #ifdef CLK + in_smp += len; + if (in_smp >= dsp->sr_base) { + double s_d = in_smp / (double)dsp->sr_base; + double t_d = 0; + struct timespec t_2; + clock_gettime(CLOCK_REALTIME, &t_2); + t_d = (t_2.tv_sec - t_1.tv_sec); + t_d += (t_2.tv_nsec - t_1.tv_nsec)/1e9; + if (t_init > 1 && t_d > 0.9) t_acc += t_d - s_d; + else t_init = 2; + + if (dsp->opt_dbg) { + fprintf(stderr, "insmp: %d dt: %.3f s_d: %.3f t_acc: %.3f\n", in_smp, t_d, s_d, t_acc); + } + + t_1 = t_2; + in_smp = 0; + } + #endif + + return len; +} + +static int f32read_cblock(dsp_t *dsp) { // blk_cond + + int n; + int len = dsp->decM; + + if (bufeof) return 0; + //if (dsp->thd->used == 0) { } + + pthread_mutex_lock( dsp->thd->mutex ); + + if (rbf == 0) + { + len = f32_cblk(dsp); + + rbf = rbf1; // set all bits + pthread_cond_broadcast( dsp->thd->cond ); + } + + while ((rbf & dsp->thd->tn_bit) == 0) pthread_cond_wait( dsp->thd->cond, dsp->thd->mutex ); + + for (n = 0; n < dsp->decM; n++) dsp->decMbuf[n] = dsp->thd->blk[dsp->decM*dsp->blk_cnt + n]; + + dsp->blk_cnt += 1; + if (dsp->blk_cnt == blk_sz) { + rbf &= ~(dsp->thd->tn_bit); // clear bit(tn) + dsp->blk_cnt = 0; + } + + pthread_mutex_unlock( dsp->thd->mutex ); + + return len; +} + +int reset_blockread(dsp_t *dsp) { + + int len = 0; + + pthread_mutex_lock( dsp->thd->mutex ); + + rbf1 &= ~(dsp->thd->tn_bit); + + if ( (rbf & dsp->thd->tn_bit) == dsp->thd->tn_bit ) + { + len = f32_cblk(dsp); + + rbf = rbf1; // set all bits + pthread_cond_broadcast( dsp->thd->cond ); + } + pthread_mutex_unlock( dsp->thd->mutex ); + + return len; +} + +// decimate lowpass +static float *ws_dec; + +static double sinc(double x) { + double y; + if (x == 0) y = 1; + else y = sin(M_PI*x)/(M_PI*x); + return y; +} + +static int lowpass_init(float f, int taps, float **pws) { + double *h, *w; + double norm = 0; + int n; + float *ws = NULL; + + if (taps % 2 == 0) taps++; // odd/symmetric + + if ( taps < 1 ) taps = 1; + + h = (double*)calloc( taps+1, sizeof(double)); if (h == NULL) return -1; + w = (double*)calloc( taps+1, sizeof(double)); if (w == NULL) return -1; + ws = (float*)calloc( 2*taps+1, sizeof(float)); if (ws == NULL) return -1; + + for (n = 0; n < taps; n++) { + w[n] = 7938/18608.0 - 9240/18608.0*cos(2*M_PI*n/(taps-1)) + 1430/18608.0*cos(4*M_PI*n/(taps-1)); // Blackmann + h[n] = 2*f*sinc(2*f*(n-(taps-1)/2)); + ws[n] = w[n]*h[n]; + norm += ws[n]; // 1-norm + } + for (n = 0; n < taps; n++) { + ws[n] /= norm; // 1-norm + } + + for (n = 0; n < taps; n++) ws[taps+n] = ws[n]; // duplicate/unwrap + + *pws = ws; + + free(h); h = NULL; + free(w); w = NULL; + + return taps; +} + +int decimate_init(float f, int taps) { + return lowpass_init(f, taps, &ws_dec); +} + +int decimate_free() { + if (ws_dec) { free(ws_dec); ws_dec = NULL; } + return 0; +} + +static float complex lowpass0(float complex buffer[], ui32_t sample, ui32_t taps, float *ws) { + ui32_t n; + double complex w = 0; + for (n = 0; n < taps; n++) { + w += buffer[(sample+n+1)%taps]*ws[taps-1-n]; + } + return (float complex)w; +} +static float complex lowpass(float complex buffer[], ui32_t sample, ui32_t taps, float *ws) { + ui32_t n; + ui32_t s = sample % taps; + double complex w = 0; + for (n = 0; n < taps; n++) { + w += buffer[n]*ws[taps+s-n]; // ws[taps+s-n] = ws[(taps+sample-n)%taps] + } + return (float complex)w; +// symmetry: ws[n] == ws[taps-1-n] +} + +/* -------------------------------------------------------------------------- */ + +int read_ifblock(dsp_t *dsp, float complex *z) { + + ui32_t s_reset = dsp->dectaps*dsp->lut_len; + int j; + + if ( f32read_cblock(dsp) < dsp->decM ) return EOF; + //if ( f32read_cblock(dsp) < dsp->decM * blk_sz) return EOF; + for (j = 0; j < dsp->decM; j++) { + dsp->decXbuffer[dsp->sample_dec % dsp->dectaps] = dsp->decMbuf[j] * dsp->ex[dsp->sample_dec % dsp->lut_len]; + dsp->sample_dec += 1; + if (dsp->sample_dec == s_reset) dsp->sample_dec = 0; + } + + *z = lowpass(dsp->decXbuffer, dsp->sample_dec, dsp->dectaps, ws_dec); + + return 0; +} + +int read_fftblock(dsp_t *dsp) { + + if ( f32read_cblock(dsp) < dsp->decM ) return EOF; + + return 0; +} + +/* -------------------------------------------------------------------------- */ + +#define IF_TRANSITION_BW (4e3) // 4kHz transition width +#define FM_TRANSITION_BW (2e3) // 2kHz transition width + + + +static double norm2_vect(float *vect, int n) { + int i; + double x, y = 0.0; + for (i = 0; i < n; i++) { + x = vect[i]; + y += x*x; + } + return y; +} + +#define HZBIN 100 + +int init_buffers(dsp_t *dsp) { + + float t; + int n, k; + + + if (dsp->thd->fft == 0) + { + // + // pcm_dec_init() + // + + // lookup table, exp-rotation + int W = 2*8; // 16 Hz window + int d = 1; // 1..W , groesster Teiler d <= W von sr_base + int freq = (int)( dsp->thd->xlt_fq * (double)dsp->sr_base + 0.5); + int freq0 = freq; // init + double f0 = freq0 / (double)dsp->sr_base; // init + + for (d = W; d > 0; d--) { // groesster Teiler d <= W von sr + if (dsp->sr_base % d == 0) break; + } + if (d == 0) d = 1; // d >= 1 ? + + for (k = 0; k < W/2; k++) { + if ((freq+k) % d == 0) { + freq0 = freq + k; + break; + } + if ((freq-k) % d == 0) { + freq0 = freq - k; + break; + } + } + + dsp->lut_len = dsp->sr_base / d; + f0 = freq0 / (double)dsp->sr_base; + + dsp->ex = calloc(dsp->lut_len+1, sizeof(float complex)); + if (dsp->ex == NULL) return -1; + for (n = 0; n < dsp->lut_len; n++) { + t = f0*(double)n; + dsp->ex[n] = cexp(t*2*M_PI*I); + } + + + dsp->decXbuffer = calloc( dsp->dectaps+1, sizeof(float complex)); + if (dsp->decXbuffer == NULL) return -1; + + } + else { + dsp->decXbuffer = NULL; + dsp->ex = NULL; + } + + + dsp->decMbuf = calloc( dsp->decM+1, sizeof(float complex)); + if (dsp->decMbuf == NULL) return -1; + + + dsp->DFT.sr = dsp->sr_base; + + int mn = 0; // 0: N = M + +/* + dsp->DFT.LOG2N = 14; + dsp->DFT.N2 = 1 << dsp->DFT.LOG2N; + if (dsp->DFT.N2 > dsp->DFT.sr/2) { + dsp->DFT.LOG2N = 0; + while ( (1 << (dsp->DFT.LOG2N+1)) < dsp->DFT.sr/2 ) dsp->DFT.LOG2N++; + dsp->DFT.N2 = 1 << dsp->DFT.LOG2N; + } +*/ + dsp->DFT.LOG2N = log(dsp->DFT.sr/HZBIN)/log(2)+0.1; + if (dsp->DFT.LOG2N < 10) dsp->DFT.LOG2N = 10; + dsp->DFT.N2 = 1 << dsp->DFT.LOG2N; + dsp->DFT.N = dsp->DFT.N2 << mn; + dsp->DFT.LOG2N += mn; +/* +if (dsp->opt_dbg && dsp->thd->fft) { +//fprintf(stderr, "HZBIN: %d , N: %d , Hz_per_bin: %.1f\n", HZBIN, dsp->DFT.N, bin2freq(&(dsp->DFT), 1)); +} +*/ + dsp->DFT.X = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.X == NULL) return -1; + dsp->DFT.Z = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.Z == NULL) return -1; + + dsp->DFT.ew = calloc(dsp->DFT.LOG2N+1, sizeof(float complex)); if (dsp->DFT.ew == NULL) return -1; + + // FFT window + // a) N2 = N + // b) N2 < N (interpolation) + dsp->DFT.win = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.win == NULL) return -1; // float real + dsp->DFT.N2 = dsp->DFT.N; + //dsp->DFT.N2 = dsp->DFT.N/2 - 1; // N=2^log2N + dft_window(&dsp->DFT, 1); + + for (n = 0; n < dsp->DFT.LOG2N; n++) { + k = 1 << n; + dsp->DFT.ew[n] = cexp(-I*M_PI/(float)k); + } + + return 0; +} + +int free_buffers(dsp_t *dsp) { + + + if (dsp->DFT.ew) { free(dsp->DFT.ew); dsp->DFT.ew = NULL; } + if (dsp->DFT.X) { free(dsp->DFT.X); dsp->DFT.X = NULL; } + if (dsp->DFT.Z) { free(dsp->DFT.Z); dsp->DFT.Z = NULL; } + + if (dsp->DFT.win) { free(dsp->DFT.win); dsp->DFT.win = NULL; } + + if (dsp->decMbuf) { free(dsp->decMbuf); dsp->decMbuf = NULL; } + + + if (dsp->decXbuffer) { free(dsp->decXbuffer); dsp->decXbuffer = NULL; } + if (dsp->ex) { free(dsp->ex); dsp->ex = NULL; } + + + return 0; +} + + diff --git a/demod/iq_svcl/iq_base.h b/demod/iq_svcl/iq_base.h new file mode 100644 index 0000000..db33108 --- /dev/null +++ b/demod/iq_svcl/iq_base.h @@ -0,0 +1,123 @@ + + +#include +#include +#include +#include +#include + +#ifndef M_PI + #define M_PI (3.1415926535897932384626433832795) +#endif + + +typedef unsigned char ui8_t; +typedef unsigned short ui16_t; +typedef unsigned int ui32_t; +typedef char i8_t; +typedef short i16_t; +typedef int i32_t; + + +#define MAX_FQ (4+1) +static int blk_sz = 32; // const + + +typedef struct { + int tn; + int tn_bit; + pthread_t tid; + pthread_mutex_t *mutex; + pthread_cond_t *cond; + //pthread_rwlock_t *lock; + int max_fq; + double xlt_fq; + float complex *blk; + int used; + int fft; +} thd_t; + + +typedef struct { + int sr; // sample_rate + int LOG2N; + int N; + int N2; + float complex *ew; + float complex *X; + float complex *Z; + float complex *win; // float real +} dft_t; + + +typedef struct { + FILE *fp; + // + int sr; // sample_rate + int bps; // bits/sample + int nch; // channels + int bps_out; + // + + // DFT + dft_t DFT; + + // decimate + int decM; + int blk_cnt; + ui32_t sr_base; + ui32_t dectaps; + ui32_t sample_dec; + ui32_t lut_len; + float complex *decXbuffer; + float complex *decMbuf; + float complex *ex; // exp_lut + + int opt_dbg; + + thd_t *thd; +} dsp_t; + + +typedef struct { + int sr; // sample_rate + int bps; // bits_sample bits/sample + int nch; // channels + int bps_out; + int opt_IFmin; + int sr_base; + int decM; + int dectaps; + FILE *fp; +} pcm_t; + + +typedef struct { + pcm_t pcm; + thd_t thd; + int fd; + char *fname; +} thargs_t; + + + +float read_wav_header(pcm_t *); + +int init_buffers(dsp_t *); +int free_buffers(dsp_t *); + +int decimate_init(float f, int taps); +int decimate_free(void); +int iq_dc_init(pcm_t *); + +int reset_blockread(dsp_t *); + +int read_ifblock(dsp_t *dsp, float complex *z); +int read_fftblock(dsp_t *dsp); + +void raw_dft(dft_t *dft, float complex *Z); +void db_power(dft_t *dft, float complex Z[], float db[]); +float bin2freq(dft_t *dft, int k); +float bin2fq(dft_t *dft, int k); + + diff --git a/demod/iq_svcl/iq_client.c b/demod/iq_svcl/iq_client.c new file mode 100644 index 0000000..93b5487 --- /dev/null +++ b/demod/iq_svcl/iq_client.c @@ -0,0 +1,195 @@ + +/* + * compile: + * + * gcc -O2 iq_client.c -o iq_client + * + * author: zilog80 + */ + +#include +#include +#include + +#include +#include + +#include "iq_svcl.h" + + +static sa_in_t serv_addr; +static int serv_port = PORT; +static int sock_fd; +static char *str_addr = "127.0.0.1"; // 0.0.0.0 + +static int send_len, recv_len; +static char sendln[LINELEN+1]; +static char recvln[LINELEN+1]; + + +int main(int argc, char *argv[]) { + + int re = 0; + char *fname_fft = "db_fft_cl.txt"; + + memset(sendln, 0, LINELEN+1); + + ++argv; + while ( *argv ) { + if (strcmp(*argv, "--ip") == 0) { + ++argv; + if (*argv) str_addr = *argv; else return -1; + } + else if (strcmp(*argv, "--port") == 0) { + int port = 0; + ++argv; + if (*argv) port = atoi(*argv); else return -1; + if (port < PORT_LO || port > PORT_HI) { + fprintf(stderr, "error: port %d..%d\n", PORT_LO, PORT_HI); + } + else serv_port = port; + } + else if (strcmp(*argv, "--fft0") == 0) { + sprintf(sendln, "%s", "--fft0"); + } + else if (strcmp(*argv, "--fft") == 0) { + sprintf(sendln, "%s", "--fft"); + ++argv; + if (*argv) { + fname_fft = *argv; + } + else return -1; + re = 2; + } + else if (strcmp(*argv, "--freq") == 0) { + ++argv; + if (*argv) { + snprintf(sendln, LINELEN, "--freq %s", *argv); + re = 1; + } + else return -1; + } + else if ((*argv)[0] == '-' && (*argv)[1] != '-') { + snprintf(sendln, LINELEN,"%s", *argv); + } + else if (strcmp(*argv, "--stop") == 0) { + sprintf(sendln, "%s", "--stop"); + } + ++argv; + } + + sock_fd = socket(AF_INET, SOCK_STREAM, 0); // TCP + if (sock_fd < 0) { + fprintf(stderr, "error: create socket\n"); + return 1; + } + + memset(&serv_addr, 0, sizeof(serv_addr)); + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(serv_port); + + if ( *sendln ) + { + if (inet_pton(AF_INET, str_addr, &serv_addr.sin_addr) <= 0) { + fprintf(stderr, "error: inet_pton %s\n", str_addr); + return 2; + } + + if ( connect(sock_fd, (sa_t *) &serv_addr, sizeof(serv_addr)) < 0 ) { + fprintf(stderr, "error: connect\n"); + return 3; + } + + send_len = strlen(sendln); + + if ( write(sock_fd, sendln, send_len) != send_len ) { + fprintf(stderr, "error: write socket\n"); + return 4; + } + + fprintf(stderr, "port: %d\n", serv_port); + + if ( re ) + { + //int count = 0; + int len; + + if ( re == 1 ) + { + // header + memset(recvln, 0, LINELEN+1); + len = HDRLEN; + while ( (recv_len = read(sock_fd, recvln, len)) > 0) { + recvln[recv_len] = '\0'; + if ( *recvln ) fprintf(stderr, "%s", recvln); + len -= recv_len; + if (len == 0) break; + } + if (recv_len < 0) { + fprintf(stderr, "error: read socket\n"); + return 5; + } + + + // + // data + + memset(recvln, 0, LINELEN+1); + + //ioctl(sock_fd, FIONREAD, &count); + while ( (recv_len = read(sock_fd, recvln, LINELEN)) > 0) { + + len = fwrite(recvln, recv_len, 1, stdout); + if (len != 1) { + fprintf(stderr, "error: write %d blocks\n", len); + break; + } + memset(recvln, 0, LINELEN+1); + //ioctl(sock_fd, FIONREAD, &count); + } + if (recv_len < 0) { + fprintf(stderr, "error: read socket\n"); + //return 5; + } + } + else if ( re == 2 ) + { + // fft data + FILE *fpo = fopen(fname_fft, "wb"); + if (fpo != NULL) { + + memset(recvln, 0, LINELEN+1); + + //ioctl(sock_fd, FIONREAD, &count); + while ( (recv_len = read(sock_fd, recvln, LINELEN)) > 0) { + + len = fwrite(recvln, recv_len, 1, fpo); + if (len != 1) { + fprintf(stderr, "error: write %d blocks\n", len); + break; + } + memset(recvln, 0, LINELEN+1); + //ioctl(sock_fd, FIONREAD, &count); + } + if (recv_len < 0) { + fprintf(stderr, "error: read socket\n"); + //return 5; + } + + fclose(fpo); + } + else { + fprintf(stderr, "error: open %s\n", fname_fft); + } + + } + } + + if ( sock_fd > 0 ) { + close(sock_fd); + } + } + + return 0; +} + diff --git a/demod/iq_svcl/iq_fm.c b/demod/iq_svcl/iq_fm.c new file mode 100644 index 0000000..3dc2aea --- /dev/null +++ b/demod/iq_svcl/iq_fm.c @@ -0,0 +1,464 @@ + +/* + * compile: + * gcc -Ofast iq_fm.c -lm -o iq_fm + * + * usage: + * ./iq_fm [--lpbw ] [- ] [--bo [--wav] [iqfile] + * --lpbw : lowpass bw in kHz, default 12.0 + * - : input raw IQ data + * --bo : bps=8,16,32 output bps + * --wav : output wav header + * [iqfile] : wav IQ-file or raw data (no iqfile: stdin) + * + * examples: + * ./iq_fm --lpbw 8.0 - 48000 16 --wav iq_data_48k16.raw > fm.wav + * cat iq_data_48k32.raw | ./iq_fm --lpbw 8.0 - 48000 32 > fm_48k32.raw + * ./iq_fm --lpbw 8.0 --wav iq_data.wav > fm.wav + * + * author: zilog80 + */ + + +#include +#include +#include +#include +#include + +#ifndef M_PI + #define M_PI (3.1415926535897932384626433832795) +#endif + +#define FM_GAIN (0.8) + + +typedef unsigned char ui8_t; +typedef unsigned short ui16_t; +typedef unsigned int ui32_t; +typedef char i8_t; +typedef short i16_t; +typedef int i32_t; + + +typedef struct { + + FILE *fp; + // + int sr; // sample_rate + int bps; // bits/sample + int nch; // channels + int bps_out; + + ui32_t sample; + + // IF: lowpass + int lpIQ_bw; + float lpIQ_fbw; + int lpIQtaps; + float *ws_lpIQ; + float complex *lpIQ_buf; + +} dsp_t; + + +typedef struct { + int sr; // sample_rate + int bps; // bits_sample bits/sample + int nch; // channels + int bps_out; + FILE *fp; +} pcm_t; + + +static int findstr(char *buff, char *str, int pos) { + int i; + for (i = 0; i < 4; i++) { + if (buff[(pos+i)%4] != str[i]) break; + } + return i; +} + +static float read_wav_header(pcm_t *pcm) { + FILE *fp = pcm->fp; + char txt[4+1] = "\0\0\0\0"; + unsigned char dat[4]; + int byte, p=0; + int sample_rate = 0, bits_sample = 0, channels = 0; + + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "RIFF", 4)) return -1; + if (fread(txt, 1, 4, fp) < 4) return -1; + // pos_WAVE = 8L + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "WAVE", 4)) return -1; + // pos_fmt = 12L + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "fmt ", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + + if (fread(dat, 1, 2, fp) < 2) return -1; + channels = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 4, fp) < 4) return -1; + memcpy(&sample_rate, dat, 4); //sample_rate = dat[0]|(dat[1]<<8)|(dat[2]<<16)|(dat[3]<<24); + + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + //byte = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 2, fp) < 2) return -1; + bits_sample = dat[0] + (dat[1] << 8); + + // pos_dat = 36L + info + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "data", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + + + fprintf(stderr, "sample_rate: %d\n", sample_rate); + fprintf(stderr, "bits : %d\n", bits_sample); + fprintf(stderr, "channels : %d\n", channels); + + + if (bits_sample != 8 && bits_sample != 16 && bits_sample != 32) return -1; + + + pcm->sr = sample_rate; + pcm->bps = bits_sample; + pcm->nch = channels; + + return 0; +} + +static float write_wav_header(pcm_t *pcm) { + FILE *fp = stdout; + ui32_t sr = pcm->sr; + ui32_t bps = pcm->bps_out; + ui32_t data = 0; + + fwrite("RIFF", 1, 4, fp); + data = 0; // bytes-8=headersize-8+datasize + fwrite(&data, 1, 4, fp); + fwrite("WAVE", 1, 4, fp); + + fwrite("fmt ", 1, 4, fp); + data = 16; if (bps == 32) data += 2; + fwrite(&data, 1, 4, fp); + + if (bps == 32) data = 3; // IEEE float + else data = 1; // PCM + fwrite(&data, 1, 2, fp); + + data = 1; // 1 channel + fwrite(&data, 1, 2, fp); + + data = sr; + fwrite(&data, 1, 4, fp); + + data = sr*bps/8; + fwrite(&data, 1, 4, fp); + + data = (bps+7)/8; + fwrite(&data, 1, 2, fp); + + data = bps; + fwrite(&data, 1, 2, fp); + + if (bps == 32) { + data = 0; // size of extension: 0 + fwrite(&data, 1, 2, fp); + } + + fwrite("data", 1, 4, fp); + data = 0; // datasize + fwrite(&data, 1, 4, fp); + + return 0; +} + +static int f32read_csample(dsp_t *dsp, float complex *z) { + + float x, y; + + if (dsp->bps == 32) { //float32 + float f[2]; + if (fread( f, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = f[0]; + y = f[1]; + } + else if (dsp->bps == 16) { //int16 + short b[2]; + if (fread( b, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = b[0]/32768.0; + y = b[1]/32768.0; + } + else { // dsp->bps == 8 //uint8 + ui8_t u[2]; + if (fread( u, dsp->bps/8, 2, dsp->fp) != 2) return EOF; + x = (u[0]-128)/128.0; + y = (u[1]-128)/128.0; + } + + *z = x + I*y; + + return 0; +} + +static double sinc(double x) { + double y; + if (x == 0) y = 1; + else y = sin(M_PI*x)/(M_PI*x); + return y; +} + +static int lowpass_init(float f, int taps, float **pws) { + double *h, *w; + double norm = 0; + int n; + float *ws = NULL; + + if (taps % 2 == 0) taps++; // odd/symmetric + + if ( taps < 1 ) taps = 1; + + h = (double*)calloc( taps+1, sizeof(double)); if (h == NULL) return -1; + w = (double*)calloc( taps+1, sizeof(double)); if (w == NULL) return -1; + ws = (float*)calloc( 2*taps+1, sizeof(float)); if (ws == NULL) return -1; + + for (n = 0; n < taps; n++) { + w[n] = 7938/18608.0 - 9240/18608.0*cos(2*M_PI*n/(taps-1)) + 1430/18608.0*cos(4*M_PI*n/(taps-1)); // Blackmann + h[n] = 2*f*sinc(2*f*(n-(taps-1)/2)); + ws[n] = w[n]*h[n]; + norm += ws[n]; // 1-norm + } + for (n = 0; n < taps; n++) { + ws[n] /= norm; // 1-norm + } + + for (n = 0; n < taps; n++) ws[taps+n] = ws[n]; // duplicate/unwrap + + *pws = ws; + + free(h); h = NULL; + free(w); w = NULL; + + return taps; +} + +static float complex lowpass0(float complex buffer[], ui32_t sample, ui32_t taps, float *ws) { + ui32_t n; + double complex w = 0; + for (n = 0; n < taps; n++) { + w += buffer[(sample+n+1)%taps]*ws[taps-1-n]; + } + return (float complex)w; +} +static float complex lowpass(float complex buffer[], ui32_t sample, ui32_t taps, float *ws) { + ui32_t n; + ui32_t s = sample % taps; + double complex w = 0; + for (n = 0; n < taps; n++) { + w += buffer[n]*ws[taps+s-n]; // ws[taps+s-n] = ws[(taps+sample-n)%taps] + } + return (float complex)w; +// symmetry: ws[n] == ws[taps-1-n] +} + +/* -------------------------------------------------------------------------- */ + +#define IF_TRANSITION_BW (4e3) // 4kHz transition width + +static int init_buffers(dsp_t *dsp) { + + float f_lp; // lowpass_bw + int taps; // lowpass taps: 4*sr/transition_bw + + // IF lowpass + f_lp = 24e3; // default + f_lp = dsp->lpIQ_bw/(float)dsp->sr/2.0; + taps = 4*dsp->sr/IF_TRANSITION_BW; if (taps%2==0) taps++; + taps = lowpass_init(f_lp, taps, &dsp->ws_lpIQ); if (taps < 0) return -1; + + dsp->lpIQ_fbw = f_lp; + dsp->lpIQtaps = taps; + dsp->lpIQ_buf = calloc( dsp->lpIQtaps+3, sizeof(float complex)); + if (dsp->lpIQ_buf == NULL) return -1; + + return 0; +} + +static int free_buffers(dsp_t *dsp) { + + if (dsp->lpIQ_buf) { free(dsp->lpIQ_buf); dsp->lpIQ_buf = NULL; } + if (dsp->ws_lpIQ) { free(dsp->ws_lpIQ); dsp->ws_lpIQ = NULL; } + + return 0; +} + +/* -------------------------------------------------------------------------- */ + +static int fm_demod(dsp_t *dsp, float *s) { + double gain = FM_GAIN; + + float complex z, z1, w; + static float complex z0; + + if ( f32read_csample(dsp, &z) == EOF ) return EOF; + + dsp->lpIQ_buf[dsp->sample % dsp->lpIQtaps] = z; + z1 = lowpass(dsp->lpIQ_buf, dsp->sample, dsp->lpIQtaps, dsp->ws_lpIQ); + + w = z1 * conj(z0); + *s = gain * carg(w)/M_PI; + + z0 = z1; + + dsp->sample += 1; + + return 0; +} + +static int write_fm(dsp_t *dsp, float s) { + int bps = dsp->bps_out; + FILE *fpo = stdout; + ui8_t u = 0; + i16_t b = 0; + ui32_t *w = (ui32_t*)&s; + + if (bps == 8) { + s *= 127.0; + s += 128.0; + u = (ui8_t)s; + w = (ui32_t*)&u; + } + else if (bps == 16) { + s *= 127.0*256.0; + b = (i16_t)s; + w = (ui32_t*)&b; + } + fwrite( w, bps/8, 1, fpo); + + return 0; +} + + +int main(int argc, char *argv[]) { + + int k; + int option_pcmraw = 0; + int option_wav = 0; + int file_loaded = 0; + + int bitQ = 0; + + float lpIQ_bw = 12e3; + float s = 0.0; + + FILE *fp; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; + + + ++argv; + while ((*argv) && (!file_loaded)) { + if (0) { } + else if (strcmp(*argv, "--lpbw") == 0) { // IQ lowpass BW / kHz + double bw = 0.0; + ++argv; + if (*argv) bw = atof(*argv); + else return 1; + if (bw > 4.6 && bw < 24.0) lpIQ_bw = bw*1e3; + } + else if (strcmp(*argv, "-") == 0) { + int sample_rate = 0, bits_sample = 0, channels = 0; + ++argv; + if (*argv) sample_rate = atoi(*argv); else return 1; + ++argv; + if (*argv) bits_sample = atoi(*argv); else return 1; + channels = 2; + if (sample_rate < 1 || (bits_sample != 8 && bits_sample != 16 && bits_sample != 32)) { + fprintf(stderr, "- \n"); + return 1; + } + pcm.sr = sample_rate; + pcm.bps = bits_sample; + pcm.nch = channels; + option_pcmraw = 1; + } + else if (strcmp(*argv, "--bo") == 0) { + int bps_out = 0; + ++argv; + if (*argv) bps_out = atoi(*argv); else return 1; + if ((bps_out != 8 && bps_out != 16 && bps_out != 32)) { + bps_out = 0; + } + pcm.bps_out = bps_out; + } + else if (strcmp(*argv, "--wav") == 0) { + option_wav = 1; + } + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "error: open %s\n", *argv); + return 1; + } + file_loaded = 1; + } + ++argv; + } + if (!file_loaded) fp = stdin; + + pcm.fp = fp; + if (option_pcmraw == 0) { + k = read_wav_header( &pcm ); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return 1; + } + } + if (pcm.nch < 2) { + fprintf(stderr, "error: iq channels < 2\n"); + return 1; + } + + if (pcm.bps_out == 0) pcm.bps_out = pcm.bps; + + if (option_wav) write_wav_header( &pcm ); + + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.bps_out = pcm.bps_out; + dsp.lpIQ_bw = lpIQ_bw; // IF lowpass bandwidth + + init_buffers(&dsp); + + + while ( 1 ) + { + bitQ = fm_demod(&dsp, &s); + if ( bitQ == EOF ) break; + + write_fm(&dsp, s); + } + + + free_buffers(&dsp); + + return 0; +} + diff --git a/demod/iq_svcl/iq_server.c b/demod/iq_svcl/iq_server.c new file mode 100644 index 0000000..0a3cb17 --- /dev/null +++ b/demod/iq_svcl/iq_server.c @@ -0,0 +1,838 @@ + +/* + * compile: + * + * gcc -Ofast -c iq_base.c + * gcc -O2 iq_server.c iq_base.o -lm -pthread -o iq_server + * + * (gcc -O2 iq_client.c -o iq_client) + * + * author: zilog80 + */ + + +#include +#include +#include + +#include // open(),close() +#include // O_RDONLY //... setmode()/cygwin + +#include + +#ifdef CYGWIN + #include +#endif + +#include "iq_svcl.h" +#include "iq_base.h" + +#define FPOUT stderr + +#define OPT_FFT_SERV 1 // server +#define OPT_FFT_CLSV 2 // server (client request) +#define OPT_FFT_CLNT 3 // server -> client + +static int option_dbg = 0; + +static int tcp_eof = 0; + +static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +//static pthread_rwlock_t lock = PTHREAD_RWLOCK_INITIALIZER; + +static float complex *block_decMB; + +extern int rbf1; // iq_base.c +extern int bufeof; // iq_base.c + +#define IF_SAMPLE_RATE 48000 +#define IF_SAMPLE_RATE_MIN 32000 + +static int pcm_dec_init(pcm_t *p) { + + int IF_sr = IF_SAMPLE_RATE; // designated IF sample rate + int decM = 1; // decimate M:1 + int sr_base = p->sr; + float f_lp; // dec_lowpass: lowpass_bandwidth/2 + float tbw; // dec_lowpass: transition_bandwidth/Hz + int taps; // dec_lowpass: taps + + if (p->opt_IFmin) IF_sr = IF_SAMPLE_RATE_MIN; + if (IF_sr > sr_base) IF_sr = sr_base; + if (IF_sr < sr_base) { + while (sr_base % IF_sr) IF_sr += 1; + decM = sr_base / IF_sr; + } + + f_lp = (IF_sr+20e3)/(4.0*sr_base); + tbw = (IF_sr-20e3)/*/2.0*/; + if (p->opt_IFmin) { + tbw = (IF_sr-12e3); + } + if (tbw < 0) tbw = 10e3; + taps = sr_base*4.0/tbw; if (taps%2==0) taps++; + + taps = decimate_init(f_lp, taps); + + if (taps < 0) return -1; + p->dectaps = (ui32_t)taps; + p->sr_base = sr_base; + p->sr = IF_sr; // sr_base/decM + p->decM = decM; + if (p->bps_out == 0) p->bps_out = p->bps; + + iq_dc_init(p); + + fprintf(stderr, "IF : %d\n", IF_sr); + fprintf(stderr, "dec: %d\n", decM); + fprintf(stderr, "bps: %d\n", p->bps_out); + + if (option_dbg) { + fprintf(stderr, "taps: %d\n", taps); + fprintf(stderr, "transBW: %.4f = %.1f Hz\n", tbw/sr_base, tbw); + fprintf(stderr, "f: +/-%.4f = +/-%.1f Hz\n", f_lp, f_lp*sr_base); + } + + return 0; +} + + +static int write_cpx(dsp_t *dsp, int fd, float complex z) { + short b[2]; + ui8_t u[2]; + float x, y; + int bps = dsp->bps_out; + + x = creal(z); + y = cimag(z); + + if (bps == 32) { + write(fd, &x, bps/8); + write(fd, &y, bps/8); + } + else { + x *= 128.0; // 127.0 + y *= 128.0; // 127.0 + if (bps == 8) { + x += 128.0; // x *= scale8b; + u[0] = (ui8_t)(x); //b = (int)(x+0.5); + y += 128.0; // x *= scale8b; + u[1] = (ui8_t)(y); //b = (int)(y+0.5); + write(fd, u, 2*bps/8); + } + else { // bps == 16 + x *= 256.0; + y *= 256.0; + b[0] = (short)x; //b = (int)(x+0.5); + b[1] = (short)y; //b = (int)(y+0.5); + write(fd, b, 2*bps/8); + } + } + + return 0; +} + +static int write_cpx_blk(dsp_t *dsp, int fd, float complex *z, int len) { + int j, l; + short b[2*len]; + ui8_t u[2*len]; + float xy[2*len]; + int bps = dsp->bps_out; + + for (j = 0; j < len; j++) { + xy[2*j ] = creal(z[j]); + xy[2*j+1] = cimag(z[j]); + } + + if (bps == 32) { + l = write(fd, xy, 2*len*bps/8); + } + else { + for (j = 0; j < 2*len; j++) xy[j] *= 128.0; // 127.0 + if (bps == 8) { + for (j = 0; j < 2*len; j++) { + xy[j] += 128.0; // x *= scale8b; + u[j] = (ui8_t)(xy[j]); //b = (int)(x+0.5); + } + l = write(fd, u, 2*len*bps/8); + } + else { // bps == 16 + for (j = 0; j < 2*len; j++) { + xy[j] *= 256.0; + b[j] = (short)xy[j]; //b = (int)(x+0.5); + } + l = write(fd, b, 2*len*bps/8); + } + } + + return l*8/(2*bps); +} + + +#define ZLEN 64 +static void *thd_IF(void *targs) { // pcm_t *pcm, double xlt_fq + + thargs_t *tharg = targs; + pcm_t *pcm = &(tharg->pcm); + + int k; + int bitQ = 0; + int n = 0; + int len = ZLEN; + int l; + + float complex z_vec[ZLEN]; + + char msg[HDRLEN]; + + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + + // init dsp + // + dsp.fp = pcm->fp; + dsp.sr = pcm->sr; + dsp.bps = pcm->bps; + dsp.nch = pcm->nch; + dsp.sr_base = pcm->sr_base; + dsp.dectaps = pcm->dectaps; + dsp.decM = pcm->decM; + + dsp.thd = &(tharg->thd); + + dsp.opt_dbg = option_dbg; + + // default: bps_out = bps_in + // bps_out < f32: IQ-dc offset (iq_client/decoder) + // bps_out = 32 recommended + dsp.bps_out = pcm->bps_out; + + + if (option_dbg) { + fprintf(stderr, "init IF buffers\n"); + } + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + goto exit_thread; + } + + + // header + // + k = 0; + memset(msg, 0, HDRLEN); + snprintf(msg, HDRLEN, "client: %d\nsr: %d\nbsp: %d\n", (dsp.thd)->tn, dsp.sr, dsp.bps_out); + k = strlen(msg); + l = write(tharg->fd, msg, HDRLEN); + + + bitQ = 0; + while ( bitQ != EOF ) + { + bitQ = read_ifblock(&dsp, z_vec+n); + n++; + if (n == len || bitQ == EOF) { + if (bitQ == EOF) n--; + l = write_cpx_blk(&dsp, tharg->fd, z_vec, n); + if (option_dbg && l != n) { + fprintf(stderr, "l: %d n: %d\n", l, n); + } + if (l <= 0) { + bitQ = read_ifblock(&dsp, z_vec+n); + (dsp.thd)->used = 0; + } + n = 0; + } + + if ( (dsp.thd)->used == 0 ) + { + pthread_mutex_lock( (dsp.thd)->mutex ); + fprintf(FPOUT, "<%d: CLOSE>\n", (dsp.thd)->tn); + pthread_mutex_unlock( (dsp.thd)->mutex ); + break; + } + } + + + free_buffers(&dsp); + + exit_thread: + + reset_blockread(&dsp); + (dsp.thd)->used = 0; + + close(tharg->fd); + + if (bitQ == EOF) { + fprintf(stderr, "<%d: EOF>\n", (dsp.thd)->tn); + } + + tcp_eof = (bitQ == EOF); + + return NULL; +} + +#define FFT_SEC 2 +#define FFT_FPS 20 + +static void *thd_FFT(void *targs) { + + thargs_t *tharg = targs; + pcm_t *pcm = &(tharg->pcm); + + FILE *fpo = NULL; + char *fname_fft = "db_fft.txt"; + + int k; + int bitQ = 0; + + float complex *z = NULL; + float *db = NULL; + float *sum_db = NULL; + + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + + // init dsp + // + dsp.fp = pcm->fp; + dsp.sr = pcm->sr; + dsp.bps = pcm->bps; + dsp.nch = pcm->nch; + dsp.sr_base = pcm->sr_base; + dsp.dectaps = pcm->dectaps; + dsp.decM = pcm->decM; + + dsp.thd = &(tharg->thd); + + dsp.opt_dbg = option_dbg; + dsp.bps_out = pcm->bps_out; + + + //(dsp.thd)->fft = 1; + if (option_dbg) { + fprintf(stderr, "init FFT buffers\n"); + } + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + goto exit_thread; + } + + + z = calloc(dsp.decM+1, sizeof(float complex)); if (z == NULL) goto exit_thread; + + db = calloc(dsp.DFT.N+1, sizeof(float)); if (db == NULL) goto exit_thread; + sum_db = calloc(dsp.DFT.N+1, sizeof(float)); if (sum_db == NULL) goto exit_thread; + + + int j, n = 0; + int len = dsp.DFT.N / dsp.decM; + int mlen = len*dsp.decM; + int sum_n = 0; + int sec = FFT_SEC; + int fft_step = dsp.sr_base/(dsp.DFT.N*FFT_FPS); + int n_fft = 0; + int th_used = 0; + int readSamples = 1; + + bitQ = 0; + while ( bitQ != EOF ) + { + #ifdef FFT_READ_SINK_MIN + //// // th_used = 0; for (j = 0; j < MAX_FQ; j++) th_used += tharg[j].thd.used; + if (rbf1 == 0) { // if (readSamples == 0 && th_used == 1) ... + readSamples = 1; + pthread_mutex_lock( (dsp.thd)->mutex ); + rbf1 |= (dsp.thd)->tn_bit; + pthread_mutex_unlock( (dsp.thd)->mutex ); + n = 0; + n_fft = 0; + sum_n = 0; + } + //// + #endif + + + if ( readSamples ) + { + bitQ = read_fftblock(&dsp); + + for (j = 0; j < dsp.decM; j++) dsp.DFT.Z[n*dsp.decM + j] = dsp.decMbuf[j]; + + n++; + if (n == len) { // mlen = len * decM <= DFT.N + + n_fft += 1; + + if ((dsp.thd)->fft && sum_n*n_fft*mlen < sec*dsp.sr_base && n_fft >= fft_step) + { + double complex dc = 0; // narrow bandwidth: no off-signal average + for (j = 0; j < mlen; j++) { + dc += dsp.DFT.Z[j]; + } + dc /= 0.99*mlen; // mlen <= dsp.DFT.N; + //dc = 0; + + for (j = 0; j < mlen; j++) { + dsp.DFT.Z[j] -= dc; + } + + for (j = 0; j < mlen; j++) { + dsp.DFT.Z[j] *= dsp.DFT.win[j]; + } + while (j < dsp.DFT.N) dsp.DFT.Z[j++] = 0.0; // dft(Z[...]) != 0 + + raw_dft(&(dsp.DFT), dsp.DFT.Z); + + db_power(&(dsp.DFT), dsp.DFT.Z, db); + for (j = 0; j < dsp.DFT.N; j++) sum_db[j] += db[j]; + + sum_n++; + n_fft = 0; + } + if (sum_n*fft_step*mlen >= sec*dsp.sr_base) { + + for (j = 0; j < dsp.DFT.N; j++) sum_db[j] /= (float)sum_n; + + pthread_mutex_lock( (dsp.thd)->mutex ); + fprintf(FPOUT, "<%d: FFT>\n", (dsp.thd)->tn); + pthread_mutex_unlock( (dsp.thd)->mutex ); + + if ( (dsp.thd)->fft == OPT_FFT_CLNT ) { // send FFT data to client + char sendln[LINELEN+1]; + int sendln_len; + int l; + snprintf(sendln, LINELEN, "# ; ## sr:%d , N:%d\n", dsp.DFT.sr, dsp.DFT.N); + sendln_len = strlen(sendln); + l = write(tharg->fd, sendln, sendln_len); + for (j = dsp.DFT.N/2; j < dsp.DFT.N/2 + dsp.DFT.N; j++) { + memset(sendln, 0, LINELEN+1); + snprintf(sendln, LINELEN, "%+11.8f;%7.2f\n", bin2fq(&(dsp.DFT), j % dsp.DFT.N), sum_db[j % dsp.DFT.N]); + sendln_len = strlen(sendln); + l = write(tharg->fd, sendln, sendln_len); + } + } + else { // save FFT at server + if ( (dsp.thd)->fft == OPT_FFT_SERV ) fname_fft = tharg->fname; + else /* OPT_FFT_CLSV */ fname_fft = "db_fft_cl.txt"; + fpo = fopen(fname_fft, "wb"); + if (fpo != NULL) { + fprintf(fpo, "# ; ## sr:%d , N:%d\n", dsp.DFT.sr, dsp.DFT.N); + for (j = dsp.DFT.N/2; j < dsp.DFT.N/2 + dsp.DFT.N; j++) { + fprintf(fpo, "%+11.8f;%7.2f\n", bin2fq(&(dsp.DFT), j % dsp.DFT.N), sum_db[j % dsp.DFT.N]); + } + fclose(fpo); + } + else { + fprintf(stderr, "error: open %s\n", fname_fft); + } + } + if ( (dsp.thd)->fft != OPT_FFT_SERV ) close(tharg->fd); + + (dsp.thd)->fft = 0; + sum_n = 0; + } + + #ifdef FFT_READ_SINK_MIN + //// + th_used = 0; + for (j = 0; j < MAX_FQ; j++) th_used += tharg[j].thd.used; + if (th_used > 1) { + readSamples = 0; + reset_blockread(&dsp); + } + //// + #endif + + n = 0; + } + } + + if ( (dsp.thd)->used == 0 ) + { + pthread_mutex_lock( (dsp.thd)->mutex ); + fprintf(FPOUT, "<%d: CLOSE>\n", (dsp.thd)->tn); + pthread_mutex_unlock( (dsp.thd)->mutex ); + break; + } + + } + + if (bitQ == EOF) { + pthread_mutex_lock( (dsp.thd)->mutex ); + fprintf(FPOUT, "<%d: EOF>\n", (dsp.thd)->tn); + pthread_mutex_unlock( (dsp.thd)->mutex ); + } + + + free_buffers(&dsp); + + exit_thread: + + if (z) { free(z); z = NULL; } + if (db) { free(db); db = NULL; } + if (sum_db) { free(sum_db); sum_db = NULL; } + + reset_blockread(&dsp); + (dsp.thd)->used = 0; + + tcp_eof = (bitQ == EOF); + + return NULL; +} + + +int main(int argc, char **argv) { + + FILE *fp = NULL; + int wavloaded = 0; + int k; + int xlt_cnt = 0; + double base_fqs[MAX_FQ]; + void *rstype[MAX_FQ]; + int option_pcmraw = 0, + option_min = 0; + char *fname_fft = "db_fft.txt"; + + // TCP + sa_in_t serv_addr; + int serv_port = PORT; + int listen_fd; + char tcp_buf[TCPBUF_LEN]; + int th_used = 0; + int tn_fft = -1; + + pcm_t pcm = {0}; + + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) +#endif + //setbuf(FPOUT, NULL); + + sigaction(SIGPIPE, &(struct sigaction){SIG_IGN}, NULL); + + + for (k = 0; k < MAX_FQ; k++) base_fqs[k] = 0.0; + + ++argv; + while ((*argv) && (!wavloaded)) { + if (strcmp(*argv, "--dbg") == 0) { + option_dbg = 1; + } + else if (strcmp(*argv, "--port") == 0) { + int port = 0; + ++argv; + if (*argv) port = atoi(*argv); else return -1; + if (port < PORT_LO || port > PORT_HI) { + fprintf(stderr, "error: port %d..%d\n", PORT_LO, PORT_HI); + } + else serv_port = port; + } + else if (strcmp(*argv, "--fft") == 0) { + if (xlt_cnt < MAX_FQ) { + base_fqs[xlt_cnt] = 0.0; + rstype[xlt_cnt] = thd_FFT; + tn_fft = xlt_cnt; + xlt_cnt++; + } + ++argv; + if (*argv) fname_fft = *argv; else return -1; + } + else if (strcmp(*argv, "-") == 0) { + int sample_rate = 0, bits_sample = 0, channels = 0; + ++argv; + if (*argv) sample_rate = atoi(*argv); else return -1; + ++argv; + if (*argv) bits_sample = atoi(*argv); else return -1; + channels = 2; + if (sample_rate < 1 || (bits_sample != 8 && bits_sample != 16 && bits_sample != 32)) { + fprintf(stderr, "- \n"); + return -1; + } + pcm.sr = sample_rate; + pcm.bps = bits_sample; + pcm.nch = channels; + option_pcmraw = 1; + } + else if (strcmp(*argv, "--bo") == 0) { + int bps_out = 0; + ++argv; + if (*argv) bps_out = atoi(*argv); else return -1; + if ((bps_out != 8 && bps_out != 16 && bps_out != 32)) { + bps_out = 0; + } + pcm.bps_out = bps_out; + } + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "error open %s\n", *argv); + return -1; + } + wavloaded = 1; + } + ++argv; + } + if (!wavloaded) fp = stdin; + + pcm.fp = fp; + if (option_pcmraw == 0) { + k = read_wav_header( &pcm ); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + } + if (pcm.nch < 2) { + fprintf(stderr, "error: iq channels < 2\n"); + return -50; + } + + pcm.opt_IFmin = option_min; + pcm_dec_init( &pcm ); + + block_decMB = calloc(pcm.decM*blk_sz+1, sizeof(float complex)); if (block_decMB == NULL) return -1; + + + thargs_t tharg[MAX_FQ]; // xlt_cnt<=MAX_FQ + for (k = 0; k < MAX_FQ; k++) tharg[k].thd.used = 0; + for (k = 0; k < MAX_FQ; k++) tharg[k].thd.fft = 0; + + for (k = 0; k < xlt_cnt; k++) { + if (k == tn_fft) { + tharg[k].thd.fft = OPT_FFT_SERV; + tharg[k].fname = fname_fft; + } + tharg[k].thd.tn = k; + tharg[k].thd.tn_bit = (1< IF (rotate from and decimate) + + tharg[k].pcm = pcm; + + rbf1 |= tharg[k].thd.tn_bit; + tharg[k].thd.used = 1; + } + + for (k = 0; k < xlt_cnt; k++) { + pthread_create(&tharg[k].thd.tid, NULL, rstype[k], &tharg[k]); + } + + + // TCP server + // + memset(&serv_addr, 0, sizeof(serv_addr)); + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = htons(INADDR_ANY); + serv_addr.sin_port = htons(serv_port); + + + listen_fd = socket(AF_INET, SOCK_STREAM, 0); // TCP + if (listen_fd < 0) { + fprintf(stderr, "error: socket\n"); + return 1; + } + + int sockopt = 1; + if ( setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(sockopt)) < 0 ) { + fprintf(stderr, "error: reuse socket\n"); + } + + + if ( bind(listen_fd, (sa_t *) &serv_addr, sizeof(serv_addr)) < 0 ) { + fprintf(stderr, "error: bind\n"); + return 2; + } + + if ( listen(listen_fd, SERV_BACKLOG) < 0 ) { + fprintf(stderr, "error: listen\n"); + return 3; + } + + + while ( 1 ) { + + int l = 0; + + fprintf(stderr, "waiting on port %d\n", serv_port); + + int conn_fd = accept(listen_fd, (sa_t *)NULL, NULL); + if (conn_fd < 0) { + fprintf(stderr, "error: connect\n"); + // + return 4; + } + + if (option_dbg) { + fprintf(stderr, "conn_fd: %d\n", conn_fd); + } + + memset(tcp_buf, 0, TCPBUF_LEN); + + l = read(conn_fd, tcp_buf, TCPBUF_LEN-1); + if (l < 0) { + fprintf(stderr, "error: read socket\n"); + return 5; + } + + if (option_dbg) { + fprintf(FPOUT, "\"%s\"\n", tcp_buf); + } + + + th_used = 0; + for (k = 0; k < MAX_FQ; k++) th_used += tharg[k].thd.used; + //if (th_used == 0) break; + + + if ( l > 1 ) { + char *freq = tcp_buf; + while (l > 1 && tcp_buf[l-1] < 0x20) l--; + tcp_buf[l] = '\0'; // remove \n, terminate string + + if (strncmp(tcp_buf, "--freq ", 7) == 0) { + freq = tcp_buf + 7; + } + else { + if ( strcmp(tcp_buf, "--stop") == 0 ) { + close(conn_fd); + for (k = 0; k < MAX_FQ; k++) tharg[k].thd.used = 0; + break; + } + else if ( strncmp(tcp_buf, "--fft", 5) == 0 ) { + char *fname_fft_cl = "db_fft_cl.txt"; + int opt_fft = strcmp(tcp_buf, "--fft0") == 0 ? OPT_FFT_CLSV : OPT_FFT_CLNT; + //close(conn_fd); + if ( !tcp_eof ) + { + if (tn_fft >= 0) { + tharg[tn_fft].thd.fft = opt_fft; + tharg[tn_fft].fname = fname_fft_cl; + tharg[tn_fft].fd = conn_fd; + } + else { + for (k = 0; k < MAX_FQ; k++) { + if (tharg[k].thd.used == 0) break; + } + if (k < MAX_FQ) { + tharg[k].thd.fft = opt_fft; + tharg[k].fname = fname_fft_cl; + tn_fft = k; + tharg[k].thd.tn = k; + tharg[k].thd.tn_bit = (1< xlt_cnt) xlt_cnt = k; + for (k = 0; k < xlt_cnt; k++) { + tharg[k].thd.max_fq = xlt_cnt; + } + } + else { + close(conn_fd); + } + } + } + else { + close(conn_fd); + } + } + else if (tcp_buf[0] == '-') { // - : close + int num = atoi(tcp_buf+1); + if (num >= 0 && num < MAX_FQ) { + if (num != tn_fft) { + tharg[num].thd.used = 0; + } + } + close(conn_fd); + } + continue; + } + + double fq = 0.0; + if (freq) fq = atof(freq); + else return -1; + if (fq < -0.5) fq = -0.5; + if (fq > 0.5) fq = 0.5; + + // find slot + for (k = 0; k < MAX_FQ; k++) { + if (tharg[k].thd.used == 0) break; + } + if (k < MAX_FQ) { + double base_fq = fq; + + tharg[k].thd.tn = k; + tharg[k].thd.tn_bit = (1<\n", k, base_fq); + pthread_mutex_unlock( &mutex ); + + k++; + if (k > xlt_cnt) xlt_cnt = k; + for (k = 0; k < xlt_cnt; k++) { + tharg[k].thd.max_fq = xlt_cnt; + } + + } + } + } + + for (k = 0; k < xlt_cnt; k++) { + pthread_join(tharg[k].thd.tid, NULL); + } + + th_used = 1; + while (th_used) { + th_used = 0; + for (k = 0; k < MAX_FQ; k++) th_used += tharg[k].thd.used; + } + + if (block_decMB) { free(block_decMB); block_decMB = NULL; } + decimate_free(); + + fclose(fp); + + close(listen_fd); + + return 0; +} + diff --git a/demod/iq_svcl/iq_svcl.h b/demod/iq_svcl/iq_svcl.h new file mode 100644 index 0000000..04a7c56 --- /dev/null +++ b/demod/iq_svcl/iq_svcl.h @@ -0,0 +1,22 @@ + + +#include +#include +#include +#include + + +#define TCPBUF_LEN 1024 +#define SERV_BACKLOG 6 + +#define LINELEN 4096 +#define HDRLEN 256 + +#define PORT 1280 +#define PORT_LO 1024 +#define PORT_HI 65353 + +typedef struct sockaddr sa_t; +typedef struct sockaddr_in sa_in_t; + + diff --git a/demod/iq_svcl/plot_fft.py b/demod/iq_svcl/plot_fft.py new file mode 100644 index 0000000..32a81d0 --- /dev/null +++ b/demod/iq_svcl/plot_fft.py @@ -0,0 +1,40 @@ + +import os +import sys +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.ticker as ticker + + +if len(sys.argv) < 2: + print("usage:") + print("\tpython %s " % sys.argv[0]) + sys.exit() + +fft_file = sys.argv[1] + +if not os.path.isfile(fft_file): + print("error: %s not found" % fft_file) + sys.exit() + + +data = np.genfromtxt( fft_file, delimiter=';', names=['fq','db'] , skip_header=1 ) + +fq = data['fq'] +db = data['db'] + +N = len(db) +m = np.mean(db) + +db5 = np.full(N, m) +for n in range(2, N-2): + db5[n] = (db[n-2]+db[n-1]+db[n]+db[n+1]+db[n+2])/5.0 + + +ax1 = plt.subplot(111) +ax1.set_xlim(fq[0], fq[-1]) +ax1.plot( fq, db, color='b', linewidth=0.06, label='fft') +ax1.plot( fq, db5, color='g', linewidth=0.4, label='fft') + +plt.show() + diff --git a/demod/iq_svcl/scan_fft.c b/demod/iq_svcl/scan_fft.c new file mode 100644 index 0000000..42d6a37 --- /dev/null +++ b/demod/iq_svcl/scan_fft.c @@ -0,0 +1,198 @@ + +#include +#include +#include +#include + + +typedef unsigned char ui8_t; +typedef unsigned short ui16_t; +typedef unsigned int ui32_t; +typedef short i16_t; +typedef int i32_t; + +#define LINELEN 1024 + +static int option_verbose = 1; + +static float *db, *intdb; +static float *fq; //, *freq; +static float *peak; + + +int main(int argc, char **argv) { + + FILE *OUT = stderr; + FILE *fpout = stdout; + FILE *fp = NULL; + + + int j, n, k; + int N; + int f0 = 0; + int sr = 0; + + int dn; + float dx; + float sympeak = 0.0; + float sympeak2 = 0.0; + float globmin = 0.0; + float globavg = 0.0; + + char line[LINELEN+1]; + char *pbuf = NULL; + char *p1, *p2; + int c; + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _setmode(_fileno(stdin), _O_BINARY); +#endif + setbuf(stdout, NULL); + + + if (argv[1] == NULL) { + fprintf(stderr, "usage:\n"); + fprintf(stderr, "\t%s \n", argv[0]); + return 1; + } + fp = fopen(argv[1], "rb"); + if (fp == NULL) { + fprintf(stderr, "error: open %s\n", argv[1]); + return 1; + } + + N = 0; + while ( (c = fgetc(fp)) != EOF) { + if (c == '\n') N++; + } + + db = calloc(N+1, sizeof(float)); if (db == NULL) return 2; + fq = calloc(N+1, sizeof(float)); if (fq == NULL) return 2; + //freq = calloc(N+1, sizeof(float)); if (freq == NULL) return 2; + + intdb = calloc(N+1, sizeof(float)); if (intdb == NULL) return 2; + peak = calloc(N+1, sizeof(float)); if (peak == NULL) return 2; + + fseek(fp, 0, SEEK_SET); + + pbuf = fgets(line, LINELEN, fp); + p1 = strstr(line, "sr:"); + if (p1) sr = atoi(p1+3); + for (n = 0; n < N; n++) { + memset(line, 0, LINELEN+1); + pbuf = fgets(line, LINELEN, fp); + p1 = strstr(line, ";"); //p2 = strstr(p1+1, ";"); + if (p1) { + fq[n] = atof(line); //freq[n] = atof(p1+1); + db[n] = atof(p1+1); //atof(p2+1); + } + } + f0 = N/2; + + globmin = 0.0; + globavg = 0.0; + float db_spike3 = 10.0; + int spike_wl3 = 3; //freq2bin(&DFT, 200); // 3 // 200 Hz + int spike_wl5 = 5; //freq2bin(&DFT, 200); // 3 // 200 Hz + //float db_spike1 = 15.0; + //int spike_wl1 = 1; //freq2bin(&DFT, 200); // 3 // 200 Hz + + dx = 200.0; + if (sr) dx = sr*(fq[f0+1]-fq[f0]); //freq[f0+1]-freq[f0]; + dn = 2*(int)(2400.0/dx)+1; // (odd/symmetric) integration width: 4800+dx Hz + if (option_verbose > 1) fprintf(stderr, "dn = %d\n", dn); + //for (j = 0; j < N; j++) db[j] /= (float)n; + + // dc-spike (N-1,)N,0,1(,2): subtract mean/avg + // spikes in general: + for (j = 0; j < N; j++) { + if ( db[j] - db[(j-spike_wl5+N)%N] > db_spike3 + && db[j] - db[(j-spike_wl3+N)%N] > db_spike3 + && db[j] - db[(j+spike_wl3+N)%N] > db_spike3 + && db[j] - db[(j+spike_wl5+N)%N] > db_spike3 + ) { + db[j] = (db[(j-spike_wl3+N)%N]+db[(j+spike_wl3+N)%N])/2.0; + } + } + + for (j = 0; j < N; j++) { + float sum = 0.0; + for (n = j-(dn-1)/2; n <= j+(dn-1)/2; n++) sum += db[(n + N) % N]; + sum /= (float)dn; + intdb[j] = sum; + globavg += sum; // <=> db[j]; + if (sum < globmin) globmin = sum; + } + globavg /= (float)N; + + if (option_verbose) fprintf(stderr, "avg=%.2f\n", globavg); + if (option_verbose) fprintf(stderr, "min=%.2f\n", globmin); + + int dn2 = 2*dn+1; + int dn3 = (int)(4000.0/dx); // (odd/symmetric) integration width: +/-4000 Hz + + int delay = (int)(24000.0/dx); // 16000 + float *bufpeak = calloc(delay+1, sizeof(float)); if (bufpeak == NULL) return -1; + int mag = 0; + int mag0 = 0; + float max_db_loc = 0.0; + int max_db_idx = 0; + float max_mag = 0.0; + + k = 0; + + for (j = 0; j < N; j++) { // j = N/2; j < N/2 + N; j % N + + sympeak = 0.0; + for (n = 1; n <= dn3; n++) { + sympeak += (db[(j+n) % N]-globmin)*(db[(j-n + N) % N]-globmin); + } + sympeak = sqrt(abs(sympeak)/(float)dn3); // globmin > min + + sympeak2 = 0.0; + for (n = 0; n <= (dn2-1)/2; n++) sympeak2 += (intdb[(j+n) % N]-globmin)*(intdb[(j-n + N) % N]-globmin); + sympeak2 = sqrt(sympeak2/(2.0*dn2)); + + peak[k % delay] = sympeak; + + mag = (sympeak - peak[(k+1)%delay])/ 3.0; // threshold 3.0 + if ( mag < 0 ) mag = 0; + + #ifdef DBG + fprintf(fpout, "%9.6f; %10.4f; %10.4f; %10.4f; %d\n", fq[j % N], intdb[j % N], sympeak, sympeak2, mag); + #endif + + if (mag0 > 0 && mag == 0) { + if ( fabs(fq[max_db_idx]) < 0.425 ) // 85% bandwidth + { + fprintf(OUT, "peak: %+11.8f = %+8.0f Hz", fq[max_db_idx], sr*fq[max_db_idx]); + fprintf(OUT, " (%.1fdB)", max_db_loc); + //fprintf(OUT, " (%.1f)", max_mag); + fprintf(OUT, "\n"); + } + } + if (mag0 == 0 && mag > 0) { + max_db_loc = sympeak; + max_db_idx = j % N; + } + if (mag > 0 && sympeak > max_db_loc) { + max_db_loc = sympeak; + max_db_idx = j % N; + max_mag = (sympeak - peak[(k+1)%delay]); + } + + mag0 = mag; + + k++; + } + + if (option_verbose) fprintf(stderr, "bin = %.0f Hz\n", sr*(fq[f0+1]-fq[f0])); //freq[f0+1]-freq[f0] + + if (bufpeak) free(bufpeak); + + fclose(fp); + + + return 0; +} +