From 59659bcb6c10c852a9c1f63d4b9e09ec39367be9 Mon Sep 17 00:00:00 2001 From: W3AXL Date: Sun, 21 Feb 2021 23:21:26 -0600 Subject: [PATCH 1/6] added basic csq functionality added an optional parameter to the config json file per channel, you can specify a "csqLeveldBFS" parameter which is the squelch threshold in dBFS (the same unit SDR sharp uses). A value of 0, or the parameter missing entirely, will result in open squelch. --- multifm/demod.c | 60 +++++++++++++++++++++------------------- multifm/demod.h | 8 +++++- multifm/fm_demod.c | 69 ++++++++++++++++++++++++++++++++-------------- multifm/fm_demod.h | 11 +++++++- multifm/receiver.c | 24 +++++++++++++++- 5 files changed, 120 insertions(+), 52 deletions(-) diff --git a/multifm/demod.c b/multifm/demod.c index bfc38e1..edd9217 100644 --- a/multifm/demod.c +++ b/multifm/demod.c @@ -87,7 +87,7 @@ aresult_t demod_thread_process(struct demod_thread *dthr, struct sample_buf *sbu dthr->nr_pcm_samples = 0; TSL_BUG_IF_FAILED(multifm_fm_demod_process(dthr->demod, dthr->filt_samp_buf, dthr->nr_fm_samples, - dthr->out_buf, &dthr->nr_pcm_samples, &nr_processed_bytes)); + dthr->out_buf, &dthr->nr_pcm_samples, &nr_processed_bytes, dthr->csq_level_dbfs)); /* x. Write out the resulting PCM samples */ if (0 > write(dthr->fifo_fd, dthr->out_buf, nr_processed_bytes)) { @@ -208,10 +208,10 @@ aresult_t _demod_fir_prepare(struct demod_thread *thr, const double *lpf_taps, s int16_t *coeffs = NULL; double f_offs = -2.0 * M_PI * (double)offset_hz / (double)sample_rate; -#ifdef _DUMP_LPF - int64_t power = 0; - double dpower = 0.0; -#endif /* defined(_DUMP_LPF) */ + #ifdef _DUMP_LPF + int64_t power = 0; + double dpower = 0.0; + #endif /* defined(_DUMP_LPF) */ size_t base = lpf_nr_taps; DIAG("Preparing LPF for offset %d Hz", offset_hz); @@ -225,45 +225,45 @@ aresult_t _demod_fir_prepare(struct demod_thread *thr, const double *lpf_taps, s goto done; } -#ifdef _DUMP_LPF - fprintf(stderr, "lpf_shifted_%d = [\n", offset_hz); -#endif /* defined(_DUMP_LPF) */ + #ifdef _DUMP_LPF + fprintf(stderr, "lpf_shifted_%d = [\n", offset_hz); + #endif /* defined(_DUMP_LPF) */ for (size_t i = 0; i < lpf_nr_taps; i++) { /* Calculate the new tap coefficient */ const double complex lpf_tap = gain * cexp(CMPLX(0, f_offs * (double)i)) * lpf_taps[i]; const double q15 = 1ll << Q_15_SHIFT; -#ifdef _DUMP_LPF - double ptemp = 0; - int64_t samp_power = 0; -#endif + #ifdef _DUMP_LPF + double ptemp = 0; + int64_t samp_power = 0; + #endif /* Calculate the Q31 coefficient */ coeffs[ i] = (int16_t)(creal(lpf_tap) * q15); coeffs[base + i] = (int16_t)(cimag(lpf_tap) * q15); -#ifdef _DUMP_LPF - ptemp = sqrt( (creal(lpf_tap) * creal(lpf_tap)) + (cimag(lpf_tap) * cimag(lpf_tap)) ); - samp_power = sqrt( ((int64_t)coeffs[i] * (int64_t)coeffs[i]) + ((int64_t)coeffs[base + i] * (int64_t)coeffs[base + i]) ); + #ifdef _DUMP_LPF + ptemp = sqrt( (creal(lpf_tap) * creal(lpf_tap)) + (cimag(lpf_tap) * cimag(lpf_tap)) ); + samp_power = sqrt( ((int64_t)coeffs[i] * (int64_t)coeffs[i]) + ((int64_t)coeffs[base + i] * (int64_t)coeffs[base + i]) ); - power += samp_power; - dpower += ptemp; + power += samp_power; + dpower += ptemp; - fprintf(stderr, " complex(%f, %f), %% (%d, %d)\n", creal(lpf_tap), cimag(lpf_tap), coeffs[i], coeffs[base + i]); -#endif /* defined(_DUMP_LPF) */ + fprintf(stderr, " complex(%f, %f), %% (%d, %d)\n", creal(lpf_tap), cimag(lpf_tap), coeffs[i], coeffs[base + i]); + #endif /* defined(_DUMP_LPF) */ } -#ifdef _DUMP_LPF - fprintf(stderr, "];\n"); - fprintf(stderr, "%% Total power: %llu (%016llx) (%f)\n", power, power, dpower); -#endif /* defined(_DUMP_LPF) */ + #ifdef _DUMP_LPF + fprintf(stderr, "];\n"); + fprintf(stderr, "%% Total power: %llu (%016llx) (%f)\n", power, power, dpower); + #endif /* defined(_DUMP_LPF) */ /* Create a Direct Type FIR implementation */ TSL_BUG_IF_FAILED(direct_fir_init(&thr->fir, lpf_nr_taps, coeffs, &coeffs[base], decimation, true, sample_rate, offset_hz)); -done: - if (NULL != coeffs) { - TFREE(coeffs); - } + done: + if (NULL != coeffs) { + TFREE(coeffs); + } return ret; } @@ -272,7 +272,8 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, int32_t offset_hz, uint32_t samp_hz, const char *out_fifo, int decimation_factor, const double *lpf_taps, size_t lpf_nr_taps, const char *fir_debug_output, - double channel_gain) + double channel_gain, + int csq_level_dbfs) { aresult_t ret = A_OK; @@ -293,6 +294,9 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, thr->fifo_fd = -1; thr->debug_signal_fd = -1; + // Pass in the CSQ variable + thr->csq_level_dbfs = csq_level_dbfs; + /* Initialize the work queue */ if (FAILED(ret = work_queue_new(&thr->wq, 128))) { goto done; diff --git a/multifm/demod.h b/multifm/demod.h index cce4400..e78cc9a 100644 --- a/multifm/demod.h +++ b/multifm/demod.h @@ -99,6 +99,11 @@ struct demod_thread { * Output demodulated sample buffer */ int16_t out_buf[LPF_OUTPUT_LEN]; + + /** + * CSQ level in dBFS (0 = open squelch) + */ + int8_t csq_level_dbfs; }; aresult_t demod_thread_delete(struct demod_thread **pthr); @@ -113,5 +118,6 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, int32_t offset_hz, uint32_t samp_hz, const char *out_fifo, int decimation_factor, const double *lpf_taps, size_t lpf_nr_taps, const char *fir_debug_output, - double channel_gain); + double channel_gain, + int csq_level_dbfs); diff --git a/multifm/fm_demod.c b/multifm/fm_demod.c index 99403f9..3afb8d6 100644 --- a/multifm/fm_demod.c +++ b/multifm/fm_demod.c @@ -1,6 +1,7 @@ #include #include #include +#include #include @@ -34,7 +35,7 @@ aresult_t multifm_fm_demod_init(struct demod_base **pdemod) } aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples, size_t nr_in_samples, - int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes) + int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes, int csq_level_dbfs) { aresult_t ret = A_OK; @@ -50,29 +51,55 @@ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples dfm = BL_CONTAINER_OF(demod, struct multifm_fm_demod, demod); + // Calculate average power of samples + float sum_smp_rms = 0; for (size_t i = 0; i < nr_in_samples; i++) { - /* Get the complex conjugate of the prior sample, negating the phase term */ - int32_t b_re = dfm->last_fm_re, - b_im = -dfm->last_fm_im, - a_re = in_samples[2 * i ], + // Calculate RMS of I & Q samples + int32_t re_smp = in_samples[2 * i]; + int32_t im_smp = in_samples[2 * i + 1]; + float smp_rms = sqrt( ( pow(re_smp,2) + pow(im_smp,2) ) / 2.0 ); + sum_smp_rms += smp_rms; + } + float avg_smp_rms = sum_smp_rms / (float)nr_in_samples; + // Convert to dBm + float avg_smp_vrms = (avg_smp_rms + SMP_OFFSET) / SMP_SCALE; + float avg_smp_wrms = pow(avg_smp_vrms,2)/50.0; + float avg_smp_dBFS = 10*log10(avg_smp_wrms); + // Debug print + //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.3f, %.3f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); + + // Demod the samples if we're above the threshold, silence if not + for (size_t i = 0; i < nr_in_samples; i++) { + // Get next samples + int32_t a_re = in_samples[2 * i ], a_im = in_samples[2 * i + 1]; - int32_t s_re = 0, - s_im = 0; - - /* Calculate the phase difference */ - s_re = a_re * b_re - a_im * b_im; - s_im = a_re * b_im + a_im * b_re; - - /* Convert from cartesian coordinates to a phase angle */ - /* TODO: This needs to be made full-integer */ - float phi = fast_atan2f((float)s_im, (float)s_re); - - /* Scale by pi (since atan2 returns an angle in (-pi,pi]), convert back to Q.15 */ - float phi_scaled = (phi/M_PI) * to_q15; - out_samples[nr_out_samples] = (int16_t)phi_scaled; - + // Check squelch and demod if above threshold or if we're in open squelch + if ((avg_smp_dBFS >= csq_level_dbfs) || csq_level_dbfs == 0) { + /* Get the complex conjugate of the prior sample, negating the phase term */ + int32_t b_re = dfm->last_fm_re, + b_im = -dfm->last_fm_im; + int32_t s_re = 0, + s_im = 0; + + /* Calculate the phase difference */ + s_re = a_re * b_re - a_im * b_im; + s_im = a_re * b_im + a_im * b_re; + + /* Convert from cartesian coordinates to a phase angle */ + /* TODO: This needs to be made full-integer */ + float phi = fast_atan2f((float)s_im, (float)s_re); + + /* Scale by pi (since atan2 returns an angle in (-pi,pi]), convert back to Q.15 */ + float phi_scaled = (phi/M_PI) * to_q15; + out_samples[nr_out_samples] = (int16_t)phi_scaled; + } + // Output silence if below threshold + else { + out_samples[nr_out_samples] = (int16_t)0; + + } + // Do this regardless nr_out_samples++; - /* Store the last sample processed */ dfm->last_fm_re = a_re; dfm->last_fm_im = a_im; diff --git a/multifm/fm_demod.h b/multifm/fm_demod.h index c9fca73..9e9ae90 100644 --- a/multifm/fm_demod.h +++ b/multifm/fm_demod.h @@ -2,6 +2,15 @@ #include +/** + * divisor to convert raw sample power to microwatts for dBm calculation, + * found experimentally using a known channel power measured via SDRSharp. + * There's probably a much better way to calculated dBFS but this works "well enough," + * and dbFS is a pretty relative measurement to begin with. + */ +#define SMP_SCALE 2300.0 +#define SMP_OFFSET -4.1 + struct demod_base; /** @@ -26,7 +35,7 @@ aresult_t multifm_fm_demod_init(struct demod_base **pdemod); * out to the real-valued PCM buffer. */ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples, size_t nr_in_samples, - int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes); + int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes, int csq_level_dbfs); /** * Cleanup the resources used by the FM demodulator diff --git a/multifm/receiver.c b/multifm/receiver.c index 97ee39b..d72b1b3 100644 --- a/multifm/receiver.c +++ b/multifm/receiver.c @@ -130,16 +130,19 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, rx->cleanup_func = cleanup_func; rx->thread_func = rx_func; + // Get sample buffer from config or default to 64 if (FAILED(ret = config_get_integer(cfg, &nr_samp_bufs, "nrSampBufs"))) { MFM_MSG(SEV_INFO, "DEFAULT-SAMP-BUFS", "Setting sample buffer count to 64"); nr_samp_bufs = 64; } + // Get sample rate if (FAILED(ret = config_get_integer(cfg, &sample_rate, "sampleRateHz"))) { MFM_MSG(SEV_INFO, "NO-SAMPLE-RATE", "Need to specify a sample rate, in Hertz."); goto done; } + // Get center freq if (FAILED(ret = config_get_integer(cfg, ¢er_freq, "centerFreqHz"))) { MFM_MSG(SEV_INFO, "NO-CENTER-FREQ", "You forgot to specify a center frequency, in Hz."); goto done; @@ -200,21 +203,39 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, double channel_gain = 1.0, channel_gain_db = 0.0; + // Var for optional CSQ mode + int csq_level_dbfs = 0; + + // FIFO location if (FAILED(ret = config_get_string(&channel, &fifo_name, "outFifo"))) { MFM_MSG(SEV_ERROR, "MISSING-FIFO-ID", "Missing output FIFO filename, aborting."); goto done; } + // Channel frequency if (FAILED(ret = config_get_integer(&channel, &nb_center_freq, "chanCenterFreq"))) { MFM_MSG(SEV_ERROR, "MISSING-CENTER-FREQ", "Missing output channel center frequency."); goto done; } + // Optional CSQ parameter + if (FAILED(ret = config_get_integer(&channel, &csq_level_dbfs, "csqLeveldBFS"))) { + MFM_MSG(SEV_INFO, "MISSING-SQUELCH", "No squelch specified, channel will be open squelch."); + } else { + if (csq_level_dbfs == 0) { + MFM_MSG(SEV_INFO, "NO-SQUELCH", "Squelch of 0 specified, channel will be open squelch."); + } else { + MFM_MSG(SEV_INFO, "CARRIER-SQUELCH", "Channel carrier squelch set to %d", csq_level_dbfs); + } + } + + // Optional debug if (!FAILED(ret = config_get_string(&channel, &signal_debug, "signalDebugFile"))) { MFM_MSG(SEV_INFO, "WRITING-SIGNAL-DEBUG", "The channel at frequency %d will have raw I/Q written to '%s'", nb_center_freq, signal_debug); } + // Optional channel gain if (!FAILED(ret = config_get_float(&channel, &channel_gain_db, "dBGain"))) { /* Convert the gain to linear units */ channel_gain = pow(10.0, channel_gain_db/10.0); @@ -227,7 +248,8 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, if (FAILED(ret = demod_thread_new(&dmt, -1, (int32_t)nb_center_freq - center_freq, sample_rate, fifo_name, decimation_factor, lpf_taps, lpf_nr_taps, signal_debug, - channel_gain))) + channel_gain, + csq_level_dbfs))) { MFM_MSG(SEV_ERROR, "FAILED-DEMOD-THREAD", "Failed to create demodulator thread, aborting."); goto done; From 6f2d1963132d8ff339bf7bcc97c0b669a9f062d5 Mon Sep 17 00:00:00 2001 From: W3AXL Date: Mon, 22 Feb 2021 08:38:28 -0600 Subject: [PATCH 2/6] removed floating point calculations from inside sample for loop should hopefully reduce the CPU usage a bit --- multifm/fm_demod.c | 12 ++++++------ multifm/fm_demod.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/multifm/fm_demod.c b/multifm/fm_demod.c index 3afb8d6..2ee877c 100644 --- a/multifm/fm_demod.c +++ b/multifm/fm_demod.c @@ -52,21 +52,21 @@ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples dfm = BL_CONTAINER_OF(demod, struct multifm_fm_demod, demod); // Calculate average power of samples - float sum_smp_rms = 0; + uint32_t sum_smp_rms = 0; for (size_t i = 0; i < nr_in_samples; i++) { // Calculate RMS of I & Q samples int32_t re_smp = in_samples[2 * i]; int32_t im_smp = in_samples[2 * i + 1]; - float smp_rms = sqrt( ( pow(re_smp,2) + pow(im_smp,2) ) / 2.0 ); + int32_t smp_rms = sqrt( ( pow(re_smp,2) + pow(im_smp,2) ) / 2.0 ); sum_smp_rms += smp_rms; } - float avg_smp_rms = sum_smp_rms / (float)nr_in_samples; + float avg_smp_rms = (float)sum_smp_rms / (float)nr_in_samples; // Convert to dBm - float avg_smp_vrms = (avg_smp_rms + SMP_OFFSET) / SMP_SCALE; - float avg_smp_wrms = pow(avg_smp_vrms,2)/50.0; + float avg_smp_vrms = ((float)avg_smp_rms + SMP_OFFSET) / SMP_SCALE; + float avg_smp_wrms = pow(avg_smp_vrms,2)/50; float avg_smp_dBFS = 10*log10(avg_smp_wrms); // Debug print - //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.3f, %.3f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); + MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); // Demod the samples if we're above the threshold, silence if not for (size_t i = 0; i < nr_in_samples; i++) { diff --git a/multifm/fm_demod.h b/multifm/fm_demod.h index 9e9ae90..f874f33 100644 --- a/multifm/fm_demod.h +++ b/multifm/fm_demod.h @@ -9,7 +9,7 @@ * and dbFS is a pretty relative measurement to begin with. */ #define SMP_SCALE 2300.0 -#define SMP_OFFSET -4.1 +#define SMP_OFFSET -3.6 struct demod_base; From 52294abdceebd2b0cd796f65e2f53b759f282d82 Mon Sep 17 00:00:00 2001 From: W3AXL Date: Mon, 22 Feb 2021 08:40:24 -0600 Subject: [PATCH 3/6] removed debug print --- multifm/fm_demod.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/multifm/fm_demod.c b/multifm/fm_demod.c index 2ee877c..6f6e21e 100644 --- a/multifm/fm_demod.c +++ b/multifm/fm_demod.c @@ -66,7 +66,7 @@ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples float avg_smp_wrms = pow(avg_smp_vrms,2)/50; float avg_smp_dBFS = 10*log10(avg_smp_wrms); // Debug print - MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); + //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); // Demod the samples if we're above the threshold, silence if not for (size_t i = 0; i < nr_in_samples; i++) { From ee1adaa0545fca7b38c84084ef2019e342f53850 Mon Sep 17 00:00:00 2001 From: W3AXL Date: Mon, 22 Feb 2021 09:19:34 -0600 Subject: [PATCH 4/6] finalized non-floating point sample loop --- multifm/fm_demod.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/multifm/fm_demod.c b/multifm/fm_demod.c index 6f6e21e..3b52862 100644 --- a/multifm/fm_demod.c +++ b/multifm/fm_demod.c @@ -52,21 +52,23 @@ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples dfm = BL_CONTAINER_OF(demod, struct multifm_fm_demod, demod); // Calculate average power of samples - uint32_t sum_smp_rms = 0; + unsigned long sum_smp_pk = 0; for (size_t i = 0; i < nr_in_samples; i++) { // Calculate RMS of I & Q samples int32_t re_smp = in_samples[2 * i]; int32_t im_smp = in_samples[2 * i + 1]; - int32_t smp_rms = sqrt( ( pow(re_smp,2) + pow(im_smp,2) ) / 2.0 ); - sum_smp_rms += smp_rms; + int32_t smp_pk = (re_smp * re_smp) + (im_smp * im_smp); + sum_smp_pk += smp_pk; } - float avg_smp_rms = (float)sum_smp_rms / (float)nr_in_samples; + // get average and rms + uint32_t avg_smp_pk = sum_smp_pk / nr_in_samples; + float avg_smp_rms = sqrt( (float)avg_smp_pk / 2.0); // Convert to dBm float avg_smp_vrms = ((float)avg_smp_rms + SMP_OFFSET) / SMP_SCALE; - float avg_smp_wrms = pow(avg_smp_vrms,2)/50; - float avg_smp_dBFS = 10*log10(avg_smp_wrms); + float avg_smp_wrms = pow(avg_smp_vrms,2)/50.0; + float avg_smp_dBFS = 10.0*log10(avg_smp_wrms); // Debug print - //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pwr: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); + //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pk: %d, rms: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_pk, avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); // Demod the samples if we're above the threshold, silence if not for (size_t i = 0; i < nr_in_samples; i++) { From b2b76dd1a39506f2bea90769e97de89067390d5e Mon Sep 17 00:00:00 2001 From: W3AXL Date: Mon, 19 Sep 2022 11:18:20 -0500 Subject: [PATCH 5/6] reverted all CSQ changes for first-attempt level-based squelch --- multifm/demod.c | 60 ++++++++++++++++++--------------------- multifm/demod.h | 8 +----- multifm/fm_demod.c | 71 ++++++++++++++-------------------------------- multifm/fm_demod.h | 11 +------ multifm/receiver.c | 24 +--------------- 5 files changed, 52 insertions(+), 122 deletions(-) diff --git a/multifm/demod.c b/multifm/demod.c index edd9217..bfc38e1 100644 --- a/multifm/demod.c +++ b/multifm/demod.c @@ -87,7 +87,7 @@ aresult_t demod_thread_process(struct demod_thread *dthr, struct sample_buf *sbu dthr->nr_pcm_samples = 0; TSL_BUG_IF_FAILED(multifm_fm_demod_process(dthr->demod, dthr->filt_samp_buf, dthr->nr_fm_samples, - dthr->out_buf, &dthr->nr_pcm_samples, &nr_processed_bytes, dthr->csq_level_dbfs)); + dthr->out_buf, &dthr->nr_pcm_samples, &nr_processed_bytes)); /* x. Write out the resulting PCM samples */ if (0 > write(dthr->fifo_fd, dthr->out_buf, nr_processed_bytes)) { @@ -208,10 +208,10 @@ aresult_t _demod_fir_prepare(struct demod_thread *thr, const double *lpf_taps, s int16_t *coeffs = NULL; double f_offs = -2.0 * M_PI * (double)offset_hz / (double)sample_rate; - #ifdef _DUMP_LPF - int64_t power = 0; - double dpower = 0.0; - #endif /* defined(_DUMP_LPF) */ +#ifdef _DUMP_LPF + int64_t power = 0; + double dpower = 0.0; +#endif /* defined(_DUMP_LPF) */ size_t base = lpf_nr_taps; DIAG("Preparing LPF for offset %d Hz", offset_hz); @@ -225,45 +225,45 @@ aresult_t _demod_fir_prepare(struct demod_thread *thr, const double *lpf_taps, s goto done; } - #ifdef _DUMP_LPF - fprintf(stderr, "lpf_shifted_%d = [\n", offset_hz); - #endif /* defined(_DUMP_LPF) */ +#ifdef _DUMP_LPF + fprintf(stderr, "lpf_shifted_%d = [\n", offset_hz); +#endif /* defined(_DUMP_LPF) */ for (size_t i = 0; i < lpf_nr_taps; i++) { /* Calculate the new tap coefficient */ const double complex lpf_tap = gain * cexp(CMPLX(0, f_offs * (double)i)) * lpf_taps[i]; const double q15 = 1ll << Q_15_SHIFT; - #ifdef _DUMP_LPF - double ptemp = 0; - int64_t samp_power = 0; - #endif +#ifdef _DUMP_LPF + double ptemp = 0; + int64_t samp_power = 0; +#endif /* Calculate the Q31 coefficient */ coeffs[ i] = (int16_t)(creal(lpf_tap) * q15); coeffs[base + i] = (int16_t)(cimag(lpf_tap) * q15); - #ifdef _DUMP_LPF - ptemp = sqrt( (creal(lpf_tap) * creal(lpf_tap)) + (cimag(lpf_tap) * cimag(lpf_tap)) ); - samp_power = sqrt( ((int64_t)coeffs[i] * (int64_t)coeffs[i]) + ((int64_t)coeffs[base + i] * (int64_t)coeffs[base + i]) ); +#ifdef _DUMP_LPF + ptemp = sqrt( (creal(lpf_tap) * creal(lpf_tap)) + (cimag(lpf_tap) * cimag(lpf_tap)) ); + samp_power = sqrt( ((int64_t)coeffs[i] * (int64_t)coeffs[i]) + ((int64_t)coeffs[base + i] * (int64_t)coeffs[base + i]) ); - power += samp_power; - dpower += ptemp; + power += samp_power; + dpower += ptemp; - fprintf(stderr, " complex(%f, %f), %% (%d, %d)\n", creal(lpf_tap), cimag(lpf_tap), coeffs[i], coeffs[base + i]); - #endif /* defined(_DUMP_LPF) */ + fprintf(stderr, " complex(%f, %f), %% (%d, %d)\n", creal(lpf_tap), cimag(lpf_tap), coeffs[i], coeffs[base + i]); +#endif /* defined(_DUMP_LPF) */ } - #ifdef _DUMP_LPF - fprintf(stderr, "];\n"); - fprintf(stderr, "%% Total power: %llu (%016llx) (%f)\n", power, power, dpower); - #endif /* defined(_DUMP_LPF) */ +#ifdef _DUMP_LPF + fprintf(stderr, "];\n"); + fprintf(stderr, "%% Total power: %llu (%016llx) (%f)\n", power, power, dpower); +#endif /* defined(_DUMP_LPF) */ /* Create a Direct Type FIR implementation */ TSL_BUG_IF_FAILED(direct_fir_init(&thr->fir, lpf_nr_taps, coeffs, &coeffs[base], decimation, true, sample_rate, offset_hz)); - done: - if (NULL != coeffs) { - TFREE(coeffs); - } +done: + if (NULL != coeffs) { + TFREE(coeffs); + } return ret; } @@ -272,8 +272,7 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, int32_t offset_hz, uint32_t samp_hz, const char *out_fifo, int decimation_factor, const double *lpf_taps, size_t lpf_nr_taps, const char *fir_debug_output, - double channel_gain, - int csq_level_dbfs) + double channel_gain) { aresult_t ret = A_OK; @@ -294,9 +293,6 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, thr->fifo_fd = -1; thr->debug_signal_fd = -1; - // Pass in the CSQ variable - thr->csq_level_dbfs = csq_level_dbfs; - /* Initialize the work queue */ if (FAILED(ret = work_queue_new(&thr->wq, 128))) { goto done; diff --git a/multifm/demod.h b/multifm/demod.h index e78cc9a..cce4400 100644 --- a/multifm/demod.h +++ b/multifm/demod.h @@ -99,11 +99,6 @@ struct demod_thread { * Output demodulated sample buffer */ int16_t out_buf[LPF_OUTPUT_LEN]; - - /** - * CSQ level in dBFS (0 = open squelch) - */ - int8_t csq_level_dbfs; }; aresult_t demod_thread_delete(struct demod_thread **pthr); @@ -118,6 +113,5 @@ aresult_t demod_thread_new(struct demod_thread **pthr, unsigned core_id, int32_t offset_hz, uint32_t samp_hz, const char *out_fifo, int decimation_factor, const double *lpf_taps, size_t lpf_nr_taps, const char *fir_debug_output, - double channel_gain, - int csq_level_dbfs); + double channel_gain); diff --git a/multifm/fm_demod.c b/multifm/fm_demod.c index 3b52862..99403f9 100644 --- a/multifm/fm_demod.c +++ b/multifm/fm_demod.c @@ -1,7 +1,6 @@ #include #include #include -#include #include @@ -35,7 +34,7 @@ aresult_t multifm_fm_demod_init(struct demod_base **pdemod) } aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples, size_t nr_in_samples, - int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes, int csq_level_dbfs) + int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes) { aresult_t ret = A_OK; @@ -51,57 +50,29 @@ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples dfm = BL_CONTAINER_OF(demod, struct multifm_fm_demod, demod); - // Calculate average power of samples - unsigned long sum_smp_pk = 0; for (size_t i = 0; i < nr_in_samples; i++) { - // Calculate RMS of I & Q samples - int32_t re_smp = in_samples[2 * i]; - int32_t im_smp = in_samples[2 * i + 1]; - int32_t smp_pk = (re_smp * re_smp) + (im_smp * im_smp); - sum_smp_pk += smp_pk; - } - // get average and rms - uint32_t avg_smp_pk = sum_smp_pk / nr_in_samples; - float avg_smp_rms = sqrt( (float)avg_smp_pk / 2.0); - // Convert to dBm - float avg_smp_vrms = ((float)avg_smp_rms + SMP_OFFSET) / SMP_SCALE; - float avg_smp_wrms = pow(avg_smp_vrms,2)/50.0; - float avg_smp_dBFS = 10.0*log10(avg_smp_wrms); - // Debug print - //MFM_MSG(SEV_INFO, "CSQ_DEBUG", "Average sample pk: %d, rms: %.2f, %.2f dBFS, calc. from %d samples", avg_smp_pk, avg_smp_rms, avg_smp_dBFS, (int)nr_in_samples); - - // Demod the samples if we're above the threshold, silence if not - for (size_t i = 0; i < nr_in_samples; i++) { - // Get next samples - int32_t a_re = in_samples[2 * i ], + /* Get the complex conjugate of the prior sample, negating the phase term */ + int32_t b_re = dfm->last_fm_re, + b_im = -dfm->last_fm_im, + a_re = in_samples[2 * i ], a_im = in_samples[2 * i + 1]; - // Check squelch and demod if above threshold or if we're in open squelch - if ((avg_smp_dBFS >= csq_level_dbfs) || csq_level_dbfs == 0) { - /* Get the complex conjugate of the prior sample, negating the phase term */ - int32_t b_re = dfm->last_fm_re, - b_im = -dfm->last_fm_im; - int32_t s_re = 0, - s_im = 0; - - /* Calculate the phase difference */ - s_re = a_re * b_re - a_im * b_im; - s_im = a_re * b_im + a_im * b_re; - - /* Convert from cartesian coordinates to a phase angle */ - /* TODO: This needs to be made full-integer */ - float phi = fast_atan2f((float)s_im, (float)s_re); - - /* Scale by pi (since atan2 returns an angle in (-pi,pi]), convert back to Q.15 */ - float phi_scaled = (phi/M_PI) * to_q15; - out_samples[nr_out_samples] = (int16_t)phi_scaled; - } - // Output silence if below threshold - else { - out_samples[nr_out_samples] = (int16_t)0; - - } - // Do this regardless + int32_t s_re = 0, + s_im = 0; + + /* Calculate the phase difference */ + s_re = a_re * b_re - a_im * b_im; + s_im = a_re * b_im + a_im * b_re; + + /* Convert from cartesian coordinates to a phase angle */ + /* TODO: This needs to be made full-integer */ + float phi = fast_atan2f((float)s_im, (float)s_re); + + /* Scale by pi (since atan2 returns an angle in (-pi,pi]), convert back to Q.15 */ + float phi_scaled = (phi/M_PI) * to_q15; + out_samples[nr_out_samples] = (int16_t)phi_scaled; + nr_out_samples++; + /* Store the last sample processed */ dfm->last_fm_re = a_re; dfm->last_fm_im = a_im; diff --git a/multifm/fm_demod.h b/multifm/fm_demod.h index f874f33..c9fca73 100644 --- a/multifm/fm_demod.h +++ b/multifm/fm_demod.h @@ -2,15 +2,6 @@ #include -/** - * divisor to convert raw sample power to microwatts for dBm calculation, - * found experimentally using a known channel power measured via SDRSharp. - * There's probably a much better way to calculated dBFS but this works "well enough," - * and dbFS is a pretty relative measurement to begin with. - */ -#define SMP_SCALE 2300.0 -#define SMP_OFFSET -3.6 - struct demod_base; /** @@ -35,7 +26,7 @@ aresult_t multifm_fm_demod_init(struct demod_base **pdemod); * out to the real-valued PCM buffer. */ aresult_t multifm_fm_demod_process(struct demod_base *demod, int16_t *in_samples, size_t nr_in_samples, - int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes, int csq_level_dbfs); + int16_t *out_samples, size_t *pnr_out_samples, size_t *pnr_out_bytes); /** * Cleanup the resources used by the FM demodulator diff --git a/multifm/receiver.c b/multifm/receiver.c index d72b1b3..97ee39b 100644 --- a/multifm/receiver.c +++ b/multifm/receiver.c @@ -130,19 +130,16 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, rx->cleanup_func = cleanup_func; rx->thread_func = rx_func; - // Get sample buffer from config or default to 64 if (FAILED(ret = config_get_integer(cfg, &nr_samp_bufs, "nrSampBufs"))) { MFM_MSG(SEV_INFO, "DEFAULT-SAMP-BUFS", "Setting sample buffer count to 64"); nr_samp_bufs = 64; } - // Get sample rate if (FAILED(ret = config_get_integer(cfg, &sample_rate, "sampleRateHz"))) { MFM_MSG(SEV_INFO, "NO-SAMPLE-RATE", "Need to specify a sample rate, in Hertz."); goto done; } - // Get center freq if (FAILED(ret = config_get_integer(cfg, ¢er_freq, "centerFreqHz"))) { MFM_MSG(SEV_INFO, "NO-CENTER-FREQ", "You forgot to specify a center frequency, in Hz."); goto done; @@ -203,39 +200,21 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, double channel_gain = 1.0, channel_gain_db = 0.0; - // Var for optional CSQ mode - int csq_level_dbfs = 0; - - // FIFO location if (FAILED(ret = config_get_string(&channel, &fifo_name, "outFifo"))) { MFM_MSG(SEV_ERROR, "MISSING-FIFO-ID", "Missing output FIFO filename, aborting."); goto done; } - // Channel frequency if (FAILED(ret = config_get_integer(&channel, &nb_center_freq, "chanCenterFreq"))) { MFM_MSG(SEV_ERROR, "MISSING-CENTER-FREQ", "Missing output channel center frequency."); goto done; } - // Optional CSQ parameter - if (FAILED(ret = config_get_integer(&channel, &csq_level_dbfs, "csqLeveldBFS"))) { - MFM_MSG(SEV_INFO, "MISSING-SQUELCH", "No squelch specified, channel will be open squelch."); - } else { - if (csq_level_dbfs == 0) { - MFM_MSG(SEV_INFO, "NO-SQUELCH", "Squelch of 0 specified, channel will be open squelch."); - } else { - MFM_MSG(SEV_INFO, "CARRIER-SQUELCH", "Channel carrier squelch set to %d", csq_level_dbfs); - } - } - - // Optional debug if (!FAILED(ret = config_get_string(&channel, &signal_debug, "signalDebugFile"))) { MFM_MSG(SEV_INFO, "WRITING-SIGNAL-DEBUG", "The channel at frequency %d will have raw I/Q written to '%s'", nb_center_freq, signal_debug); } - // Optional channel gain if (!FAILED(ret = config_get_float(&channel, &channel_gain_db, "dBGain"))) { /* Convert the gain to linear units */ channel_gain = pow(10.0, channel_gain_db/10.0); @@ -248,8 +227,7 @@ aresult_t receiver_init(struct receiver *rx, struct config *cfg, if (FAILED(ret = demod_thread_new(&dmt, -1, (int32_t)nb_center_freq - center_freq, sample_rate, fifo_name, decimation_factor, lpf_taps, lpf_nr_taps, signal_debug, - channel_gain, - csq_level_dbfs))) + channel_gain))) { MFM_MSG(SEV_ERROR, "FAILED-DEMOD-THREAD", "Failed to create demodulator thread, aborting."); goto done; From edcae82a55ed63a0497a9952bb75a1b0f9ab5a00 Mon Sep 17 00:00:00 2001 From: W3AXL Date: Mon, 19 Sep 2022 21:08:18 -0500 Subject: [PATCH 6/6] first stab at noise-based CSQ --- CMakeLists.txt | 1 + squelch/CMakeLists.txt | 20 ++ squelch/squelch.c | 476 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 497 insertions(+) create mode 100644 squelch/CMakeLists.txt create mode 100644 squelch/squelch.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 20691b1..090ab18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,7 @@ add_subdirectory(multifm) add_subdirectory(pager) add_subdirectory(ais) add_subdirectory(resampler) +add_subdirectory(squelch) add_subdirectory(decoder) # Only include RF interface modules we built as dpkg deps diff --git a/squelch/CMakeLists.txt b/squelch/CMakeLists.txt new file mode 100644 index 0000000..b8a8a24 --- /dev/null +++ b/squelch/CMakeLists.txt @@ -0,0 +1,20 @@ +add_executable(squelch + squelch.c + ) + +target_include_directories(squelch PUBLIC + "${TSL_SDR_BASE_DIR}" + "${TSL_INCLUDE_DIRS}") + +install(TARGETS squelch + DESTINATION ${INSTALL_BIN_DIR}) + +target_link_libraries(squelch + filter + tsltestframework + tslapp + tslconfig + tsl + pthread + m + jansson) \ No newline at end of file diff --git a/squelch/squelch.c b/squelch/squelch.c new file mode 100644 index 0000000..c6c841f --- /dev/null +++ b/squelch/squelch.c @@ -0,0 +1,476 @@ +/* + * squelch.c - Basic implementations of various FM squelch algorithms + * + * Copyright (c)2022 Patrick McDonnell + * + * This file is a part of The Standard Library (TSL) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define SQL_MSG(sev, sys, msg, ...) MESSAGE("SQUELCH", sev, sys, msg, ##__VA_ARGS__) + +#define NR_SAMPLES 1024 + +#define SQL_MAX 2000 +#define SQL_SMOOTH 0.5 + +/* Starting Globals */ +static unsigned samplerate = 0; +static unsigned sql_mode = 0; +static unsigned sql_level = 5; +static int in_fifo = -1; +static int out_fifo = -1; +static char truncate_silence = 0; +static char suppress_output = 0; +static char print_debug = 0; + +/* FIR highpass filter */ +static int16_t *filter_coeffs = NULL; /* Int16 filter coeffs array */ +static size_t nr_filter_coeffs = 0; /* Size of the filter array */ +static struct polyphase_fir *hpfir = NULL; /* Highpass FIR for noise squelch */ + +/* Coefficients for FIR Highpass filter with stop-band of 3500 and pass-band of 4000 */ +double hpf_coeffs_12k[] = { 1.5584662332383325e-21,5.427393716672668e-06,-2.3243021132657304e-05,1.1183839895825805e-19,0.00015437717956956476,-0.0003212937153875828,3.3852275578865277e-18,0.0010956734186038375,-0.0018515291158109903,3.380739069087662e-18,0.0046401359140872955,-0.0069726635701954365,8.649661402163887e-18,0.014577680267393589,-0.02050068788230419,1.6144567088675172e-17,0.039694104343652725,-0.0559057742357254,2.3112900350583163e-17,0.13085311651229858,-0.2721134126186371,0.33333516120910645,-0.2721134126186371,0.13085313141345978,2.3112900350583163e-17,-0.055905781686306,0.03969409316778183,1.6144563779952722e-17,-0.020500682294368744,0.01457767840474844,8.6496622293445e-18,-0.006972667761147022,0.004640133585780859,3.3807388622925087e-18,-0.0018515285337343812,0.001095673767849803,3.385227764681681e-18,-0.0003212936280760914,0.0001543772959848866,1.118385540546229e-19,-2.324273737031035e-05,5.427200903795892e-06,1.5584662332383325e-21 }; +double hpf_coeffs_16k[] = { -6.5822916894831e-07,4.4969875949837086e-21,9.429295459995046e-06,4.651412360265842e-20,-5.0012375140795484e-05,1.3963747072902472e-19,0.00016808522923383862,-1.3798665087091616e-18,-0.0004515335022006184,8.545128830396759e-19,0.0010446899104863405,-1.6771297850568837e-18,-0.0021634085569530725,2.9497450895181854e-18,0.004111547954380512,-4.733236239278975e-18,-0.00730621162801981,7.01368343899121e-18,0.012336397543549538,-9.678628589246163e-18,-0.020126793533563614,1.2513076281903032e-17,0.0324372872710228,-1.522132599158636e-17,-0.05364722013473511,1.7473492299745715e-17,0.09979074448347092,-1.896741191887976e-17,-0.31615251302719116,0.49999964237213135,-0.31615251302719116,-1.8967413573240985e-17,0.09979074448347092,1.7473492299745715e-17,-0.05364722013473511,-1.522132599158636e-17,0.0324372872710228,1.2513076281903032e-17,-0.020126791670918465,-9.678632725149226e-18,0.012336392886936665,7.013681784629985e-18,-0.007306211162358522,-4.7332354120983625e-18,0.004111548885703087,2.949745916698798e-18,-0.002163409488275647,-1.677130922430226e-18,0.0010446907253935933,8.545125728469462e-19,-0.0004515336186159402,-1.3798654747333959e-18,0.00016808536020107567,1.396374836537218e-19,-5.00122805533465e-05,4.651440148364545e-20,9.429262718185782e-06,4.4967331400101205e-21,-6.5822916894831e-07 }; +double hpf_coeffs_20k[] = { -5.04288948377507e-07,2.645731938592674e-21,4.654988060792675e-06,6.753271463821875e-06,-1.341389179287944e-05,-3.905207267962396e-05,1.0163967559578549e-19,0.00010553650645306334,0.00010070658754557371,-0.00015061954036355019,-0.00035472316085360944,6.492759313803158e-19,0.0007018938777036965,0.0005926831508986652,-0.0007963005336932838,-0.001705068745650351,2.2927422055420704e-18,0.0028692837804555893,0.0022604551631957293,-0.0028521502390503883,-0.005769688170403242,5.5278859062918236e-18,0.008809749037027359,0.006663785316050053,-0.008116543292999268,-0.015940677374601364,9.947648403963722e-18,0.02340324968099594,0.01757539063692093,-0.021490370854735374,-0.043000973761081696,1.3957443646442088e-17,0.07051049172878265,0.05993451550602913,-0.09191156178712845,-0.3013980984687805,0.6000003218650818,-0.3013980984687805,-0.09191156178712845,0.05993451550602913,0.07051049172878265,1.3957443646442088e-17,-0.043000977486371994,-0.021490370854735374,0.017575392499566078,0.02340325340628624,9.947647576783109e-18,-0.015940675511956215,-0.008116541430354118,0.006663783453404903,0.008809749037027359,5.527885079111211e-18,-0.0057696872390806675,-0.002852149773389101,0.002260454697534442,0.0028692844789475203,2.292743239517836e-18,-0.0017050692113116384,-0.0007963007083162665,0.0005926833255216479,0.0007018940523266792,6.492762932718338e-19,-0.000354723451891914,-0.00015061955491546541,0.00010070660209748894,0.00010553659376455471,1.0163966267108842e-19,-3.905177072738297e-05,-1.3413786291494034e-05,6.753287379979156e-06,4.654896656575147e-06,2.6459353006231526e-21,-5.04288948377507e-07 }; +double hpf_coeffs_24k[] = { -3.844478158043785e-07,1.7731983620686205e-21,2.5393778741999995e-06,5.643806161970133e-06,-6.378321848736773e-20,-1.8571487089502625e-05,-3.01547406706959e-05,7.93776797596149e-20,7.005011866567656e-05,0.00010193629714194685,-8.192116579292684e-19,-0.00020140365813858807,-0.00027502336888574064,2.037828922195838e-18,0.00048887322191149,0.0006382971769198775,-4.424679259232913e-18,-0.0010497424518689513,-0.00132482941262424,1.8735295526420336e-18,0.002052097115665674,0.0025222161784768105,-3.045806194439428e-18,-0.0037281340919435024,-0.004488963168114424,4.56014249224766e-18,0.0064026364125311375,0.007593767251819372,-2.4749389511374397e-17,-0.010571172460913658,-0.012427469715476036,8.254359232050308e-18,0.017136316746473312,0.02015301026403904,-1.0088413864704933e-17,-0.02822515182197094,-0.03381172940135002,1.1619126477485337e-17,0.05101900175213814,0.06558297574520111,-1.263688536726752e-17,-0.13613708317279816,-0.274813175201416,0.6666663885116577,-0.274813175201416,-0.13613708317279816,-1.2636886194448132e-17,0.06558298319578171,0.05101900175213814,1.1619126477485337e-17,-0.03381172940135002,-0.02822515182197094,-1.0088414691885546e-17,0.02015301026403904,0.017136313021183014,8.25435675050847e-18,-0.012427471578121185,-0.010571173392236233,-2.4749399437541748e-17,0.007593766786158085,0.006402634549885988,4.5601433194282724e-18,-0.0044889613054692745,-0.0037281345576047897,-3.045804953668509e-18,0.002522216411307454,0.0020520968828350306,1.873530173027493e-18,-0.0013248290633782744,-0.0010497428011149168,-4.4246796728232196e-18,0.0006382976425811648,0.0004888733965344727,2.037828508605532e-18,-0.000275023456197232,-0.00020140355627518147,-8.192112960377504e-19,0.00010193629714194685,7.005004590610042e-05,7.937736956688519e-20,-3.015455513377674e-05,-1.8571603504824452e-05,-6.378317971327652e-20,5.643831627821783e-06,2.5392901079612784e-06,1.7731983620686205e-21,-3.844478158043785e-07 }; + +/** + * @brief Integer-based square root + * + * Borrowed from https://stackoverflow.com/questions/1100090/looking-for-an-efficient-integer-square-root-algorithm-for-arm-thumb2 + * + * @param value input value + * @return uint32_t output integer square root + */ +static uint32_t square_root(uint32_t value) { + uint32_t op = value; + uint32_t res = 0; + uint32_t one = 1uL << 30; + + while (one > op) { + one >>= 2; + } + + while (one != 0) { + if (op >= res + one) { + op = op - (res + one); + res = res + 2 * one; + } + res >>= 1; + one >>= 2; + } + + if (op > res) { + res++; + } + + return res; +} + +/** + * @brief Return the unitless power value of an array of samples + * + * this is just sqrt(sum(abs(samples))) but it seems to work well + * + * @param samples pointer to array of samples + * @param size length of array + * @return uint16_t RMS value in uint16 format + */ +static uint16_t get_pow(int16_t *samples, uint16_t size) { + uint32_t sum = 0; + for (uint16_t i=0;i argc) { + SQL_MSG(SEV_FATAL, "MISSING-SRC-DEST", "Missing input fifo"); + exit(EXIT_FAILURE); + } + /* Verify min sample rate (necessary for our CSQ noise filtering) */ + if (!samplerate) { + SQL_MSG(SEV_FATAL, "NO-SAMPLERATE", "Missing samplerate parameter -S"); + exit(EXIT_FAILURE); + } + /* Verify squelch mode */ + if (sql_mode != 0) { + SQL_MSG(SEV_FATAL, "BAD-SQL-MODE", "Invalid squelch mode specified: %u", sql_mode); + exit(EXIT_FAILURE); + } + /* Verify squelch level */ + if (sql_level > 10 || sql_level < 0) { + SQL_MSG(SEV_FATAL, "BAD-SQL-LEVEL", "Invalid squelch level specified: %u", sql_level); + exit(EXIT_FAILURE); + } + + /* Init FIR filter based on specified sample rate */ + SQL_MSG(SEV_INFO, "FILTER-COEFFS", "Loading filter coefficients"); + double *filter_coeffs_f; + switch (samplerate) { + case 12000: + filter_coeffs_f = hpf_coeffs_12k; + nr_filter_coeffs = sizeof(hpf_coeffs_12k)/sizeof(hpf_coeffs_12k[0]); + break; + case 16000: + filter_coeffs_f = hpf_coeffs_16k; + nr_filter_coeffs = sizeof(hpf_coeffs_16k)/sizeof(hpf_coeffs_16k[0]); + break; + case 20000: + filter_coeffs_f = hpf_coeffs_20k; + nr_filter_coeffs = sizeof(hpf_coeffs_20k)/sizeof(hpf_coeffs_20k[0]); + break; + case 24000: + filter_coeffs_f = hpf_coeffs_24k; + nr_filter_coeffs = sizeof(hpf_coeffs_24k)/sizeof(hpf_coeffs_24k[0]); + break; + default: + SQL_MSG(SEV_FATAL, "FILTER-COEFFS", "Invalid sample rate specified: %d", samplerate); + exit(EXIT_FAILURE); + } + + /* Convert float FIR coeffs to int16 */ + SQL_MSG(SEV_INFO, "FILTER-COEFFS", "Loaded filter with %lu coefficients", nr_filter_coeffs); + SQL_MSG(SEV_INFO, "FILTER-COEFFS", "Converting coefficients to fixed-point"); + TSL_BUG_IF_FAILED(TCALLOC((void **)&filter_coeffs, sizeof(int16_t) * nr_filter_coeffs, (size_t)1)); + for (size_t i = 0; i < nr_filter_coeffs; i++) { + double q15 = 1 << Q_15_SHIFT; + filter_coeffs[i] = (int16_t)(filter_coeffs_f[i] * q15); + } + + /* Init FIR filter */ + polyphase_fir_new(&hpfir, nr_filter_coeffs, filter_coeffs, 1, 1); + + /* Verify & open input fifo if valid */ + if (0 > (in_fifo = open(argv[optind], O_RDONLY))) { + SQL_MSG(SEV_FATAL, "INV-IN-FIFO", "Cannot open input FIFO: %s", argv[optind]); + exit(EXIT_FAILURE); + } + /* Open our output fifo if specified */ + if (out_fifo_path) { + if (0 > (out_fifo = open(out_fifo_path, O_WRONLY))) { + SQL_MSG(SEV_FATAL, "INV-OUT-FIFO", "Cannot open output FIFO: %s", out_fifo_path); + exit(EXIT_FAILURE); + } + } +} + +/** + * @brief Free up a sample buffer + * + * @param buf + * @return aresult_t + */ +static aresult_t _free_sample_buf(struct sample_buf *buf) +{ + TSL_BUG_ON(NULL == buf); + TFREE(buf); + return A_OK; +} + +/** + * @brief Allocate a sample buffer + * + * @param pbuf + * @return aresult_t + */ +static aresult_t _alloc_sample_buf(struct sample_buf **pbuf) +{ + aresult_t ret = A_OK; + + struct sample_buf *buf = NULL; + + TSL_ASSERT_ARG(NULL != pbuf); + + if (FAILED(ret = TCALLOC((void **)&buf, NR_SAMPLES * sizeof(int16_t) + sizeof(struct sample_buf), 1ul))) { + goto done; + } + + buf->refcount = 0; + buf->sample_type = COMPLEX_INT_16; + buf->sample_buf_bytes = NR_SAMPLES * sizeof(int16_t); + buf->nr_samples = 0; + buf->release = _free_sample_buf; + buf->priv = NULL; + + *pbuf = buf; + +done: + return ret; +} + +/* Buffer for filtered samples */ +static int16_t filter_buf[NR_SAMPLES]; + +/* Array of zeroes for silence */ +static int16_t zero_buf[NR_SAMPLES] = {0}; + +/* Running squelch average */ +static int16_t squelch_avg; + +/* Char (bool-ish) for squelch state */ +static char squelched = 0; + +/* Processing Loop */ +static aresult_t process_sql(void) +{ + int ret = A_OK; + + do { + int op_ret = 0; + struct sample_buf *read_buf = NULL; + size_t new_samples = 0; + bool full = false; + + TSL_BUG_IF_FAILED(polyphase_fir_full(hpfir, &full)); + + if (false == full) { + TSL_BUG_IF_FAILED(_alloc_sample_buf(&read_buf)); + + if (0 >= (op_ret = read(in_fifo, read_buf->data_buf, read_buf->sample_buf_bytes))) { + int errnum = errno; + ret = A_E_INVAL; + SQL_MSG(SEV_FATAL, "READ-FIFO-FAIL", "Failed to read from input fifo: %s (%d)", + strerror(errnum), errnum); + goto done; + } + + DIAG("Read %d bytes from input FIFO", op_ret); + + TSL_BUG_ON((1 & op_ret) != 0); + + read_buf->nr_samples = op_ret/sizeof(int16_t); + + TSL_BUG_IF_FAILED(polyphase_fir_push_sample_buf(hpfir, read_buf)); + } + + //SQL_MSG(SEV_INFO, "INPUT", "Read %d bytes from input", op_ret); + + /* Filter the samples */ + TSL_BUG_IF_FAILED(polyphase_fir_process(hpfir, filter_buf, NR_SAMPLES, &new_samples)); + TSL_BUG_ON(0 == new_samples); + + /* Don't run squelch routine if CSQ is set to zero */ + if (!sql_level) { + squelched = 0; + } else { + // Calculate power of filtered samples and add to running average + squelch_avg = (uint16_t)((1 - SQL_SMOOTH) * squelch_avg) + (uint16_t)(get_pow(filter_buf, NR_SAMPLES) * SQL_SMOOTH / 125); + if (squelch_avg > SQL_MAX) {squelch_avg = SQL_MAX;} + if (print_debug) { + SQL_MSG(SEV_INFO, "SQL_CALC", "Squelch average: %u", squelch_avg); + } + // Squelch or unsquelch the audio + if (squelch_avg >= (10-sql_level)) { + squelched = 1; + } else { + if (print_debug) { + SQL_MSG(SEV_INFO, "SQL", "Unsquelched"); + } + squelched = 0; + } + } + + /* Don't output anything unless suppression is disabled */ + if (suppress_output == 0) { + if (squelched) { + /* If we're truncating silence, do nothing, otherwise output silence */ + if (!truncate_silence) { + /* Write to stdout */ + if (out_fifo < 0) { + if (0 > (op_ret = write(STDOUT_FILENO, zero_buf, read_buf->nr_samples * sizeof(int16_t)))) { + int errnum = errno; + ret = A_E_INVAL; + SQL_MSG(SEV_FATAL, "WRITE-STDOUT-FAIL", "Failed to write to stdout: %s (%d)", + strerror(errnum), errnum); + goto done; + } + /* Write to output fifo */ + } else { + if (0 > (op_ret = write(out_fifo, zero_buf, read_buf->nr_samples * sizeof(int16_t)))) { + int errnum = errno; + ret = A_E_INVAL; + SQL_MSG(SEV_FATAL, "WRITE-FIFO-FAIL", "Failed to write to output fifo: %s (%d)", + strerror(errnum), errnum); + goto done; + } + DIAG("Wrote %d bytes to output FIFO", op_ret); + } + } + } else { + /* Write the read samples back to the output */ + if (out_fifo < 0) { + /* Write to stdout */ + if (0 > (op_ret = write(STDOUT_FILENO, read_buf->data_buf, read_buf->nr_samples * sizeof(int16_t)))) { + int errnum = errno; + ret = A_E_INVAL; + SQL_MSG(SEV_FATAL, "WRITE-STDOUT-FAIL", "Failed to write to stdout: %s (%d)", + strerror(errnum), errnum); + goto done; + } + } else { + /* Write to output fifo */ + if (0 > (op_ret = write(out_fifo, read_buf->data_buf, read_buf->nr_samples * sizeof(int16_t)))) { + int errnum = errno; + ret = A_E_INVAL; + SQL_MSG(SEV_FATAL, "WRITE-FIFO-FAIL", "Failed to write to output fifo: %s (%d)", + strerror(errnum), errnum); + goto done; + } + DIAG("Wrote %d bytes to output FIFO", op_ret); + } + } + } + + //SQL_MSG(SEV_INFO, "OUTPUT", "Wrote %lu bytes to output", read_buf->nr_samples * sizeof(int16_t)); + + } while (app_running()); + + done: + return ret; +} + +/** + * @brief Main runtime for squelch runtime + * + * @param argc number of arguments (input fifo, ) + * @param argv arguments passed + * @return int + */ +int main(int argc, char *argv[]) +{ + /* Starting return code */ + int ret = EXIT_FAILURE; + + TSL_BUG_IF_FAILED(app_init("squelch", NULL)); + TSL_BUG_IF_FAILED(app_sigint_catch(NULL)); + + /* Get Arguments */ + _set_options(argc, argv); + + /* Print Arguments */ + SQL_MSG(SEV_INFO, "SETUP", "Configured squelch parameters:"); + SQL_MSG(SEV_INFO, "SETUP", " - Samplerate: %u", samplerate); + SQL_MSG(SEV_INFO, "SETUP", " - Squelch Mode: %i (%s)", sql_mode, _mode_name(sql_mode)); + SQL_MSG(SEV_INFO, "SETUP", " - Squelch Level: %u", sql_level); + if (truncate_silence) { SQL_MSG(SEV_INFO, "SETUP", " - Truncated silence"); } + if (suppress_output) { SQL_MSG(SEV_INFO, "SETUP", " - Suppressing output samples"); } + + SQL_MSG(SEV_INFO, "MAIN", "Starting sample processing"); + + if (FAILED(process_sql())) { + SQL_MSG(SEV_FATAL, "SQL-FAILED", "Failed during squelch processing"); + goto done; + } + + done: + return ret; +} \ No newline at end of file