mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-11-16 13:21:50 -05:00
81 lines
3.4 KiB
C++
81 lines
3.4 KiB
C++
|
///////////////////////////////////////////////////////////////////////////////////
|
||
|
|
// Copyright (C) 2005,2007,2012 Free Software Foundation, Inc.
|
||
|
|
// Copyright (C) 2020 Jon Beniston, M7RCE //
|
||
|
|
// //
|
||
|
|
// 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 as version 3 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 V3 for more details. //
|
||
|
|
// //
|
||
|
|
// You should have received a copy of the GNU General Public License //
|
||
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
#define _USE_MATH_DEFINES
|
||
|
|
#include <cmath>
|
||
|
|
#include <QDebug>
|
||
|
|
#include "dsp/fmpreemphasis.h"
|
||
|
|
|
||
|
|
FMPreemphasis::FMPreemphasis(int sampleRate, Real tau, Real highFreq)
|
||
|
|
{
|
||
|
|
configure(sampleRate, tau, highFreq);
|
||
|
|
}
|
||
|
|
|
||
|
|
void FMPreemphasis::configure(int sampleRate, Real tau, Real highFreq)
|
||
|
|
{
|
||
|
|
// Based on: https://github.com/gnuradio/gnuradio/blob/master/gr-analog/python/analog/fm_emph.py
|
||
|
|
// Compare to freq response in https://www.mathworks.com/help/comm/ref/comm.fmbroadcastmodulator-system-object.html
|
||
|
|
|
||
|
|
// High frequency corner at which to flatten the gain
|
||
|
|
double fh = std::min((double)highFreq, 0.925 * sampleRate/2.0);
|
||
|
|
|
||
|
|
// Digital corner frequencies
|
||
|
|
double w_cl = 1.0 / tau;
|
||
|
|
double w_ch = 2.0 * M_PI * fh;
|
||
|
|
|
||
|
|
// Prewarped analog corner frequencies
|
||
|
|
double w_cla = 2.0 * sampleRate * std::tan(w_cl / (2.0 * sampleRate));
|
||
|
|
double w_cha = 2.0 * sampleRate * std::tan(w_ch / (2.0 * sampleRate));
|
||
|
|
|
||
|
|
// Resulting digital pole, zero, and gain term from the bilinear
|
||
|
|
// transformation of H(s) = (s + w_cla) / (s + w_cha) to
|
||
|
|
// H(z) = b0 (1 - z1 z^-1)/(1 - p1 z^-1)
|
||
|
|
double kl = -w_cla / (2.0 * sampleRate);
|
||
|
|
double kh = -w_cha / (2.0 * sampleRate);
|
||
|
|
double z1 = (1.0 + kl) / (1.0 - kl);
|
||
|
|
double p1 = (1.0 + kh) / (1.0 - kh);
|
||
|
|
double b0 = (1.0 - kl) / (1.0 - kh);
|
||
|
|
|
||
|
|
// Adjust with a gain, g, so 0 dB gain at DC
|
||
|
|
double g = std::abs(1.0 - p1) / (b0 * std::abs(1.0 - z1));
|
||
|
|
|
||
|
|
// Caclulate IIR taps
|
||
|
|
m_b0 = (Real)(g * b0 * 1.0);
|
||
|
|
m_b1 = (Real)(g * b0 * -z1);
|
||
|
|
m_a1 = (Real)-p1;
|
||
|
|
// Zero delay line so we get reproducible results
|
||
|
|
m_z = 0;
|
||
|
|
|
||
|
|
qDebug() << "FMPreemphasis::configure: tau: " << tau
|
||
|
|
<< " sampleRate: " << sampleRate
|
||
|
|
<< " b0: " << m_b0
|
||
|
|
<< " b1: " << m_b1
|
||
|
|
<< " a1: " << m_a1;
|
||
|
|
}
|
||
|
|
|
||
|
|
Real FMPreemphasis::filter(const Real sampleIn)
|
||
|
|
{
|
||
|
|
Real sampleOut;
|
||
|
|
|
||
|
|
// See Transposed Direct form 2 - https://en.wikipedia.org/wiki/Digital_biquad_filter
|
||
|
|
sampleOut = sampleIn * m_b0 + m_z;
|
||
|
|
m_z = sampleIn * m_b1 + sampleOut * -m_a1;
|
||
|
|
|
||
|
|
return sampleOut;
|
||
|
|
}
|