kopia lustrzana https://github.com/rs1729/RS
IF-IQ server/client: test -> master
Zilog80
rodzic
d8fda5c97c
commit
690464aec9
|
|
@ -0,0 +1,47 @@
|
|||
|
||||
## IQ server/client
|
||||
|
||||
receive IF stream from baseband IQ via TCP, default `PORT=1280 (iq_svcl.h)`<br />
|
||||
|
||||
|
||||
#### Compile
|
||||
`gcc -Ofast -c iq_base.c` <br />
|
||||
`gcc -O2 iq_server.c iq_base.o -lm -pthread -o iq_server`<br />
|
||||
`gcc -O2 iq_client.c -o iq_client` <br />
|
||||
|
||||
#### Usage/Examples
|
||||
- `T1$ ./iq_server [--port <pn>] <iq_baseband.wav>`<br />
|
||||
`T2$ ./iq_client [--ip <ip_adr>] [--port <pn>] --freq <fq>`
|
||||
|
||||
- Ex.1<br />
|
||||
[terminal 1]<br />
|
||||
`T1$ ./iq_server --bo 32 <iq_baseband.wav>`
|
||||
(or `$ ./iq_server --bo 32 - <sr> <bs> <iq_baseband.raw>`)<br />
|
||||
[terminal 2]<br />
|
||||
`T2$ ./iq_client --freq <fq> | ./rs41mod -vx --IQ 0.0 --lp - <if_sr> <bo>` <br />
|
||||
where <br />
|
||||
`-0.5 < fq < 0.5`: (relative) frequency, `fq=frequency/sr` <br />
|
||||
`<if_sr>`: IF sample rate <br />
|
||||
`<bo>=8,16,32`: output/IF bits per (real) sample (u8, s16 or f32) <br />
|
||||
down-converts up to `MAX_FQ=(4+1) (iq_base.h)` channels/signals. More signals than number of CPUs/cores is not recommended.<br />
|
||||
(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.)<br />
|
||||
One channel can be used for scanning, `--fft <fft.txt>` makes FFT (2 seconds average).
|
||||
The FFT is saved in `<fft.txt>` as `<fq>;<dB>`, approx. 200 Hz per bin.<br />
|
||||
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.<br />
|
||||
|
||||
- Ex.2<br />
|
||||
[terminal 1]<br />
|
||||
`T1$ rtl_sdr -f 403.0M -s 1920000 - | ./iq_server --fft fft_server.txt --bo 32 - 1920000 8`<br />
|
||||
[terminal 2]<br />
|
||||
`T2$ ./iq_client --freq -0.3125 | ./m10mod -c -vv --IQ 0.0 - 48000 32`<br />
|
||||
[terminal 3]<br />
|
||||
`T3$ ./iq_client --freq 0.0 | ./rs41mod -vx --IQ 0.0 - 48000 32`<br />
|
||||
[terminal 4]<br />
|
||||
`T4$ ./iq_client -1` (*close channel 1*)<br />
|
||||
`T4$ ./iq_client --stop` (*close all clients and stop server*)<br />
|
||||
|
||||
The `iq_server` `--fft` option immediately starts reading the IQ stream (so buffering is reduced).<br />
|
||||
`./iq_client --fft <fft_cl.txt>` can also request FFT.<br />
|
||||
The IF sample rate `if_sr` is at least 48000 such that the baseband sample rate `sr` is a multiple of `if_sr`.
|
||||
|
||||
|
|
@ -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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#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 <time.h>
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <complex.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#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);
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
|
||||
/*
|
||||
* compile:
|
||||
*
|
||||
* gcc -O2 iq_client.c -o iq_client
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,464 @@
|
|||
|
||||
/*
|
||||
* compile:
|
||||
* gcc -Ofast iq_fm.c -lm -o iq_fm
|
||||
*
|
||||
* usage:
|
||||
* ./iq_fm [--lpbw <lp>] [- <sr> <bps>] [--bo <bps_out> [--wav] [iqfile]
|
||||
* --lpbw <lp> : lowpass bw in kHz, default 12.0
|
||||
* - <sr> <bps> : input raw IQ data
|
||||
* --bo <bps_out> : 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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <complex.h>
|
||||
|
||||
#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, "- <sr> <bs>\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;
|
||||
}
|
||||
|
||||
|
|
@ -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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <unistd.h> // open(),close()
|
||||
#include <fcntl.h> // O_RDONLY //... setmode()/cygwin
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
#ifdef CYGWIN
|
||||
#include <io.h>
|
||||
#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, "# <freq/sr>;<dB> ## 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, "# <freq/sr>;<dB> ## 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, "- <sr> <bs>\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<<k);
|
||||
tharg[k].thd.mutex = &mutex;
|
||||
tharg[k].thd.cond = &cond;
|
||||
//tharg[k].thd.lock = &lock;
|
||||
tharg[k].thd.blk = block_decMB;
|
||||
tharg[k].thd.max_fq = xlt_cnt;
|
||||
tharg[k].thd.xlt_fq = -base_fqs[k]; // S(t)*exp(-f*2pi*I*t): fq baseband -> 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<<k);
|
||||
tharg[k].thd.mutex = &mutex;
|
||||
tharg[k].thd.cond = &cond;
|
||||
//tharg[k].thd.lock = &lock;
|
||||
tharg[k].thd.blk = block_decMB;
|
||||
tharg[k].thd.xlt_fq = 0.0;
|
||||
|
||||
tharg[k].pcm = pcm;
|
||||
tharg[k].fd = conn_fd;
|
||||
|
||||
rbf1 |= tharg[k].thd.tn_bit;
|
||||
tharg[k].thd.used = 1;
|
||||
|
||||
pthread_create(&tharg[k].thd.tid, NULL, thd_FFT, &tharg[k]);
|
||||
|
||||
k++;
|
||||
if (k > 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] == '-') { // -<n> : close <n>
|
||||
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<<k);
|
||||
tharg[k].thd.mutex = &mutex;
|
||||
tharg[k].thd.cond = &cond;
|
||||
//tharg[k].thd.lock = &lock;
|
||||
tharg[k].thd.blk = block_decMB;
|
||||
tharg[k].thd.xlt_fq = -base_fq;
|
||||
|
||||
tharg[k].pcm = pcm;
|
||||
tharg[k].fd = conn_fd;
|
||||
|
||||
rbf1 |= tharg[k].thd.tn_bit;
|
||||
tharg[k].thd.used = 1;
|
||||
tharg[k].thd.fft = 0;
|
||||
|
||||
pthread_create(&tharg[k].thd.tid, NULL, thd_IF, &tharg[k]);
|
||||
|
||||
pthread_mutex_lock( &mutex );
|
||||
fprintf(FPOUT, "<%d: ADD f=%+.4f>\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;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
|
||||
#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;
|
||||
|
||||
|
||||
|
|
@ -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 <fft_file>" % 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()
|
||||
|
||||
|
|
@ -0,0 +1,198 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
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 <fft_file>\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;
|
||||
}
|
||||
|
||||
Ładowanie…
Reference in New Issue