From b0b9e3d24fc39a67f186e19746658ade01632589 Mon Sep 17 00:00:00 2001 From: Kyle Keen Date: Wed, 24 Oct 2012 17:33:32 +0200 Subject: [PATCH] rtl_fm: WBFM, AM, LSB and USB demod, raw IQ mode Signed-off-by: Steve Markgraf --- src/rtl_fm.c | 227 +++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 194 insertions(+), 33 deletions(-) diff --git a/src/rtl_fm.c b/src/rtl_fm.c index df7d768..c185dec 100644 --- a/src/rtl_fm.c +++ b/src/rtl_fm.c @@ -27,10 +27,11 @@ * remove float math (disqualifies complex.h) * replace atan2 with a fast approximation * in-place array operations - * wide band support + * better wide band support * sanity checks * nicer FIR than square * scale squelch to other input parameters + * test all the demodulations */ #include @@ -56,8 +57,8 @@ #define DEFAULT_SAMPLE_RATE 24000 #define DEFAULT_ASYNC_BUF_NUMBER 32 #define DEFAULT_BUF_LENGTH (1 * 16384) -#define MINIMAL_BUF_LENGTH 512 -#define MAXIMAL_BUF_LENGTH (256 * 16384) +#define MAXIMUM_OVERSAMPLE 16 +#define MAXIMUM_BUF_LENGTH (MAXIMUM_OVERSAMPLE * DEFAULT_BUF_LENGTH) #define CONSEQ_SQUELCH 20 #define AUTO_GAIN -100 @@ -65,6 +66,7 @@ static pthread_t demod_thread; static sem_t data_ready; static int do_exit = 0; static rtlsdr_dev_t *dev = NULL; +static int lcm_post[17] = {1,1,1,3,1,5,3,7,1,9,5,11,3,13,7,15,1}; struct fm_state { @@ -79,21 +81,28 @@ struct fm_state int squelch_level; int squelch_hits; int term_squelch_hits; - uint8_t buf[DEFAULT_BUF_LENGTH]; + uint8_t buf[MAXIMUM_BUF_LENGTH]; uint32_t buf_len; - int signal[DEFAULT_BUF_LENGTH]; /* 16 bit signed i/q pairs */ - int16_t signal2[DEFAULT_BUF_LENGTH]; /* signal has lowpass, signal2 has demod */ + int signal[MAXIMUM_BUF_LENGTH]; /* 16 bit signed i/q pairs */ + int16_t signal2[MAXIMUM_BUF_LENGTH]; /* signal has lowpass, signal2 has demod */ int signal_len; + int signal2_len; FILE *file; int edge; uint32_t freqs[100]; int freq_len; int freq_now; uint32_t sample_rate; + int output_rate; int fir_enable; int fir[256]; /* fir_len == downsample */ int fir_sum; int custom_atan; + int deemph; + int deemph_a; + int now_lpr; + int prev_lpr_index; + void (*mode_demod)(struct fm_state*); }; void usage(void) @@ -107,13 +116,23 @@ void usage(void) "\t[-d device_index (default: 0)]\n" "\t[-g tuner_gain (default: automatic)]\n" "\t[-l squelch_level (default: 150)]\n" - "\t[-E freq sets lower edge (default: center)]\n" + "\t[-o oversampling (default: 1)]\n" + "\t[-E sets lower edge tuning (default: center)]\n" + "\t[-N enables NBFM mode (default: on)]\n" + "\t[-W enables WBFM mode (default: off)]\n" + "\t (-N -s 170e3 -o 4 -A -r 16e3 -l 0 -D)\n" "\tfilename (a '-' dumps samples to stdout)\n\n" "Experimental options:\n" + "\t[-r output rate (default: same as -s)]\n" "\t[-t terminate_hits (default: 0/off)]\n" "\t (requires squelch on and scanning off)\n" - "\t[-o oversampling (default: 1) !!BROKEN!!]\n" + "\t[-M enables AM mode (default: off)]\n" + "\t[-L enables LSB mode (default: off)]\n" + "\t[-U enables USB mode (default: off)]\n" + //"\t[-D enables DSB mode (default: off)]\n" + "\t[-R enables raw mode (default: off, 2x16 bit output)]\n" "\t[-F enables high quality FIR (default: off/square)]\n" + "\t[-D enables de-emphasis (default: off)]\n" "\t[-A enables high speed arctan (default: off)]\n\n" "Produces signed 16 bit ints, use Sox to hear them.\n" "\trtl_fm ... - | play -t raw -r 24k -e signed-integer -b 16 -c 1 -V1 -\n\n"); @@ -171,12 +190,12 @@ void low_pass(struct fm_state *fm, unsigned char *buf, uint32_t len) fm->now_j += ((int)buf[i+1] - 128); i += 2; fm->prev_index++; - if (fm->prev_index < (fm->downsample)) { + if (fm->prev_index < fm->downsample) { continue; } fm->signal[i2] = fm->now_r * fm->output_scale; fm->signal[i2+1] = fm->now_j * fm->output_scale; - fm->prev_index = -1; + fm->prev_index = 0; fm->now_r = 0; fm->now_j = 0; i2 += 2; @@ -209,17 +228,17 @@ void low_pass_fir(struct fm_state *fm, unsigned char *buf, uint32_t len) { int i=0, i2=0, i3=0; while (i < (int)len) { - fm->prev_index++; i3 = fm->prev_index; fm->now_r += ((int)buf[i] - 128) * fm->fir[i3] * fm->downsample / fm->fir_sum; fm->now_j += ((int)buf[i+1] - 128) * fm->fir[i3] * fm->downsample / fm->fir_sum; i += 2; - if (fm->prev_index < (fm->downsample)) { + fm->prev_index++; + if (fm->prev_index < fm->downsample) { continue; } fm->signal[i2] = fm->now_r * fm->output_scale; fm->signal[i2+1] = fm->now_j * fm->output_scale; - fm->prev_index = -1; + fm->prev_index = 0; fm->now_r = 0; fm->now_j = 0; i2 += 2; @@ -236,12 +255,35 @@ int low_pass_simple(int16_t *signal2, int len, int step) for(i2=0; i2sample_rate / fm->post_downsample; + int slow = fm->output_rate; + while (i < fm->signal2_len) { + fm->now_lpr += fm->signal2[i]; + i++; + fm->prev_lpr_index += slow; + if (fm->prev_lpr_index < fast) { + continue; + } + fm->signal2[i2] = (int16_t)(fm->now_lpr / (fast/slow)); + fm->prev_lpr_index -= fast; + fm->now_lpr = 0; + i2 += 1; + } + fm->signal2_len = i2; +} + /* define our own complex math ops because ARMv5 has no hardware float */ @@ -257,8 +299,6 @@ int polar_discriminant(int ar, int aj, int br, int bj) double angle; multiply(ar, aj, br, -bj, &cr, &cj); angle = atan2((double)cj, (double)cr); - //if (angle > (3.14159) || angle < (-3.14159)) - // {fprintf(stderr, "overflow %f\n", angle);} return (int)(angle / 3.14159 * (1<<14)); } @@ -310,6 +350,69 @@ void fm_demod(struct fm_state *fm) } fm->pre_r = fm->signal[fm->signal_len - 2]; fm->pre_j = fm->signal[fm->signal_len - 1]; + fm->signal2_len = fm->signal_len/2; +} + +void am_demod(struct fm_state *fm) +// todo, fix this extreme laziness +{ + int i, pcm; + for (i = 0; i < (fm->signal_len); i += 2) { + // hypot uses floats but won't overflow + //pcm = (int)hypot(fm->signal[i], fm->signal[i+1]); + pcm = fm->signal[i] * fm->signal[i]; + pcm += fm->signal[i+1] * fm->signal[i+1]; + fm->signal2[i/2] = (int16_t)sqrt(pcm); // * fm->output_scale; + } + fm->signal2_len = fm->signal_len/2; + // lowpass? (3khz) highpass? (dc) +} + +void usb_demod(struct fm_state *fm) +{ + int i, pcm; + for (i = 0; i < (fm->signal_len); i += 2) { + pcm = fm->signal[i] + fm->signal[i+1]; + fm->signal2[i/2] = (int16_t)pcm; // * fm->output_scale; + } + fm->signal2_len = fm->signal_len/2; +} + +void lsb_demod(struct fm_state *fm) +{ + int i, pcm; + for (i = 0; i < (fm->signal_len); i += 2) { + pcm = fm->signal[i] - fm->signal[i+1]; + fm->signal2[i/2] = (int16_t)pcm; // * fm->output_scale; + } + fm->signal2_len = fm->signal_len/2; +} + +void raw_demod(struct fm_state *fm) +{ + /* hacky and pointless code */ + int i; + for (i = 0; i < (fm->signal_len); i++) { + fm->signal2[i] = (int16_t)fm->signal[i]; + } + fm->signal2_len = fm->signal_len; +} + +void deemph_filter(struct fm_state *fm) +{ + static int avg; // cheating... + int i, d; + // de-emph IIR + // avg = avg * (1 - alpha) + sample * alpha; + for (i = 0; i < fm->signal2_len; i++) { + d = fm->signal2[i] - avg; + if (d > 0) { + avg += (d + fm->deemph_a/2) / fm->deemph_a; + } else { + avg += (d - fm->deemph_a/2) / fm->deemph_a; + } + fm->signal2[i] = (int16_t)avg; + } } int mad(int *samples, int len, int step) @@ -371,7 +474,7 @@ static void optimal_settings(struct fm_state *fm, int freq, int hopping) fprintf(stderr, "Oversampling input by: %ix.\n", fm->downsample); fprintf(stderr, "Oversampling output by: %ix.\n", fm->post_downsample); fprintf(stderr, "Buffer size: %0.2fms\n", - 1000 * 0.5 * (float)DEFAULT_BUF_LENGTH / (float)capture_rate); + 1000 * 0.5 * lcm_post[fm->post_downsample] * (float)DEFAULT_BUF_LENGTH / (float)capture_rate); if (r < 0) { fprintf(stderr, "WARNING: Failed to set center freq.\n");} else { @@ -379,27 +482,40 @@ static void optimal_settings(struct fm_state *fm, int freq, int hopping) /* Set the sample rate */ fprintf(stderr, "Sampling at %u Hz.\n", capture_rate); + if (fm->output_rate > 0) { + fprintf(stderr, "Output at %u Hz.\n", fm->output_rate); + } else { + fprintf(stderr, "Output at %u Hz.\n", fm->sample_rate/fm->post_downsample);} r = rtlsdr_set_sample_rate(dev, (uint32_t)capture_rate); if (r < 0) { fprintf(stderr, "WARNING: Failed to set sample rate.\n");} } -void full_demod(unsigned char *buf, uint32_t len, struct fm_state *fm) +void full_demod(struct fm_state *fm) { int sr, freq_next; - rotate_90(buf, len); + rotate_90(fm->buf, fm->buf_len); if (fm->fir_enable) { - low_pass_fir(fm, buf, len); + low_pass_fir(fm, fm->buf, fm->buf_len); } else { - low_pass(fm, buf, len); + low_pass(fm, fm->buf, fm->buf_len); + } + fm->mode_demod(fm); + if (fm->mode_demod == &raw_demod) { + fwrite(fm->signal2, 2, fm->signal2_len, fm->file); + return; } - fm_demod(fm); sr = post_squelch(fm); if (fm->post_downsample > 1) { - fm->signal_len = low_pass_simple(fm->signal2, fm->signal_len/2, fm->post_downsample)*2;} + fm->signal2_len = low_pass_simple(fm->signal2, fm->signal2_len, fm->post_downsample);} + if (fm->output_rate > 0) { + low_pass_real(fm); + } + if (fm->deemph) { + deemph_filter(fm);} /* ignore under runs for now */ - fwrite(fm->signal2, 2, fm->signal_len/2, fm->file); + fwrite(fm->signal2, 2, fm->signal2_len, fm->file); if (fm->freq_len > 1 && !sr && fm->squelch_hits > CONSEQ_SQUELCH) { freq_next = (fm->freq_now + 1) % fm->freq_len; optimal_settings(fm, freq_next, 1); @@ -418,10 +534,10 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) return;} if (!ctx) { return;} - /* single threaded uses 25% less CPU? */ - /* full_demod(buf, len, fm2); */ memcpy(fm2->buf, buf, len); fm2->buf_len = len; + /* single threaded uses 25% less CPU? */ + /* full_demod(fm2); */ sem_getvalue(&data_ready, &dr_val); if (!dr_val) { sem_post(&data_ready);} @@ -432,7 +548,7 @@ static void *demod_thread_fn(void *arg) struct fm_state *fm2 = arg; while (!do_exit) { sem_wait(&data_ready); - full_demod(fm2->buf, fm2->buf_len, fm2); + full_demod(fm2); if (!fm2->term_squelch_hits) { continue;} if (fm2->squelch_hits > fm2->term_squelch_hits) { @@ -449,8 +565,7 @@ int main(int argc, char **argv) #endif struct fm_state fm; char *filename = NULL; - int n_read; - int r, opt; + int n_read, r, opt, wb_mode = 0; int i, gain = AUTO_GAIN; // tenths of a dB uint8_t *buffer; uint32_t dev_index = 0; @@ -463,12 +578,15 @@ int main(int argc, char **argv) fm.freq_len = 0; fm.edge = 0; fm.fir_enable = 0; - fm.prev_index = -1; + fm.prev_index = 0; fm.post_downsample = 1; // once this works, default = 4 fm.custom_atan = 0; + fm.deemph = 0; + fm.output_rate = -1; // flag for disabled + fm.mode_demod = &fm_demod; sem_init(&data_ready, 0, 0); - while ((opt = getopt(argc, argv, "d:f:g:s:b:l:o:t:EFA")) != -1) { + while ((opt = getopt(argc, argv, "d:f:g:s:b:l:o:t:r:EFANWMULRD")) != -1) { switch (opt) { case 'd': dev_index = atoi(optarg); @@ -486,8 +604,13 @@ int main(int argc, char **argv) case 's': fm.sample_rate = (uint32_t)atof(optarg); break; + case 'r': + fm.output_rate = (int)atof(optarg); + break; case 'o': fm.post_downsample = (int)atof(optarg); + if (fm.post_downsample < 1 || fm.post_downsample > MAXIMUM_OVERSAMPLE) { + fprintf(stderr, "Oversample must be between 1 and %i\n", MAXIMUM_OVERSAMPLE);} break; case 't': fm.term_squelch_hits = (int)atof(optarg); @@ -501,6 +624,34 @@ int main(int argc, char **argv) case 'A': fm.custom_atan = 1; break; + case 'D': + fm.deemph = 1; + break; + case 'N': + fm.mode_demod = &fm_demod; + break; + case 'W': + wb_mode = 1; + fm.mode_demod = &fm_demod; + fm.sample_rate = 170000; + fm.output_rate = 16000; + fm.custom_atan = 1; + fm.post_downsample = 4; + fm.deemph = 1; + fm.squelch_level = 0; + break; + case 'M': + fm.mode_demod = &am_demod; + break; + case 'U': + fm.mode_demod = &usb_demod; + break; + case 'L': + fm.mode_demod = &lsb_demod; + break; + case 'R': + fm.mode_demod = &raw_demod; + break; default: usage(); break; @@ -515,7 +666,7 @@ int main(int argc, char **argv) filename = argv[optind]; } - buffer = malloc(DEFAULT_BUF_LENGTH * sizeof(uint8_t)); + buffer = malloc(lcm_post[fm.post_downsample] * DEFAULT_BUF_LENGTH * sizeof(uint8_t)); device_count = rtlsdr_get_device_count(); if (!device_count) { @@ -550,6 +701,15 @@ int main(int argc, char **argv) SetConsoleCtrlHandler( (PHANDLER_ROUTINE) sighandler, TRUE ); #endif + /* WBFM is special */ + if (wb_mode) { + fm.freqs[0] += 15000; + } + + if (fm.deemph) { + fm.deemph_a = (int)round(1.0/((1.0-exp(-1.0/(fm.output_rate * 75e-6))))); + } + optimal_settings(&fm, 0, 0); build_fir(&fm); @@ -585,7 +745,8 @@ int main(int argc, char **argv) pthread_create(&demod_thread, NULL, demod_thread_fn, (void *)(&fm)); rtlsdr_read_async(dev, rtlsdr_callback, (void *)(&fm), - DEFAULT_ASYNC_BUF_NUMBER, DEFAULT_BUF_LENGTH); + DEFAULT_ASYNC_BUF_NUMBER, + lcm_post[fm.post_downsample] * DEFAULT_BUF_LENGTH); if (do_exit) { fprintf(stderr, "\nUser cancel, exiting...\n");}