diff options
Diffstat (limited to 'plugins/channel/nfm/nfmdemod.cpp')
-rw-r--r-- | plugins/channel/nfm/nfmdemod.cpp | 225 |
1 files changed, 176 insertions, 49 deletions
diff --git a/plugins/channel/nfm/nfmdemod.cpp b/plugins/channel/nfm/nfmdemod.cpp index 359bf09..a3c6d69 100644 --- a/plugins/channel/nfm/nfmdemod.cpp +++ b/plugins/channel/nfm/nfmdemod.cpp @@ -17,9 +17,11 @@ #include <QTime> #include <stdio.h> +#include <complex.h> #include "nfmdemod.h" #include "audio/audiooutput.h" #include "dsp/dspcommands.h" +#include "dsp/pidcontroller.h" MESSAGE_CLASS_DEFINITION(NFMDemod::MsgConfigureNFMDemod, Message) @@ -27,20 +29,17 @@ NFMDemod::NFMDemod(AudioFifo* audioFifo, SampleSink* sampleSink) : m_sampleSink(sampleSink), m_audioFifo(audioFifo) { - m_rfBandwidth = 12500; - m_volume = 2.0; - m_squelchLevel = pow(10.0, -40.0 / 20.0); - m_sampleRate = 500000; - m_frequency = 0; - m_squelchLevel *= m_squelchLevel; + m_config.m_inputSampleRate = 500000; + m_config.m_inputFrequencyOffset = 0; + m_config.m_rfBandwidth = 12500; + m_config.m_afBandwidth = 3000; + m_config.m_squelch = -40.0; + m_config.m_volume = 2.0; + m_config.m_audioSampleRate = 44100; - m_nco.setFreq(m_frequency, m_sampleRate); - m_interpolator.create(16, m_sampleRate, 12500); - m_sampleDistanceRemain = (Real)m_sampleRate / 44100.0; + apply(); - m_lowpass.create(21, 44100, 3000); - - m_audioBuffer.resize(256); + m_audioBuffer.resize(16384); m_audioBufferFill = 0; m_movingAverage.resize(16, 0); @@ -56,52 +55,145 @@ void NFMDemod::configure(MessageQueue* messageQueue, Real rfBandwidth, Real afBa cmd->submit(messageQueue, this); } +float arctan2(Real y, Real x) +{ + Real coeff_1 = M_PI / 4; + Real coeff_2 = 3 * coeff_1; + Real abs_y = fabs(y) + 1e-10; // kludge to prevent 0/0 condition + Real angle; + if( x>= 0) { + Real r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + Real r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + if(y < 0) + return(-angle); + else return(angle); +} + +Real angleDist(Real a, Real b) +{ + Real dist = b - a; + + while(dist <= M_PI) + dist += 2 * M_PI; + while(dist >= M_PI) + dist -= 2 * M_PI; + + return dist; +} + void NFMDemod::feed(SampleVector::const_iterator begin, SampleVector::const_iterator end, bool firstOfBurst) { Complex ci; bool consumed; - for(SampleVector::const_iterator it = begin; it < end; ++it) { + if(m_audioFifo->size() <= 0) + return; + + if(m_audioFifo->getSampleRate() != m_config.m_audioSampleRate) { + m_config.m_audioSampleRate = m_audioFifo->getSampleRate(); + apply(); + } + + if(firstOfBurst) { + if(m_audioFifo->isStopped()) { + if(m_audioFifo->fill() >= m_audioFifo->size() / 8.0) { + m_audioFifo->setStopped(false); + m_interpolatorRegulation = 0.9999; + } + } else { + Real err = (Real)m_audioFifo->fill() / ((Real)m_audioFifo->size() / 9.0); + if(err < 0.999) + err = 0.999; + else if(err > 1.001) + err = 1.001; + + m_interpolatorRegulation = (m_interpolatorRegulation * 400.0 + err * 1.0) / 401.0; + + if(m_interpolatorRegulation < 0.99) + m_interpolatorRegulation = 0.99; + else if(m_interpolatorRegulation > 1.01) + m_interpolatorRegulation = 1.01; + } + m_interpolatorDistance = m_interpolatorRegulation * (Real)m_running.m_inputSampleRate / (Real)m_running.m_audioSampleRate; + } + + for(SampleVector::const_iterator it = begin; it != end; ++it) { Complex c(it->real() / 32768.0, it->imag() / 32768.0); c *= m_nco.nextIQ(); consumed = false; - if(m_interpolator.interpolate(&m_sampleDistanceRemain, c, &consumed, &ci)) { - m_sampleBuffer.push_back(Sample(ci.real() * 32768.0, ci.imag() * 32768.0)); + while(!consumed) { + if(m_interpolator.interpolate(&m_interpolatorDistanceRemain, c, &consumed, &ci)) { + m_sampleBuffer.push_back(Sample(ci.real() * 32767.0, ci.imag() * 32767.0)); + + m_movingAverage.feed(ci.real() * ci.real() + ci.imag() * ci.imag()); + if(m_movingAverage.average() >= m_squelchLevel) + m_squelchState = m_running.m_audioSampleRate/ 20; + + qint16 sample; + + m_squelchState = 999; + if(m_squelchState > 0) { + m_squelchState--; + /* + Real argument = arg(ci); + Real demod = argument - m_lastArgument; + m_lastArgument = argument; + */ + + Complex d = conj(m_lastSample) * ci; + m_lastSample = ci; + Real demod = atan2(d.imag(), d.real()); + //Real demod = arctan2(d.imag(), d.real()); +/* + Real argument1 = arg(ci);//atan2(ci.imag(), ci.real()); + Real argument2 = m_lastSample.real(); + Real demod = angleDist(argument2, argument1); + m_lastSample = Complex(argument1, 0); +*/ - m_movingAverage.feed(ci.real() * ci.real() + ci.imag() * ci.imag()); - if(m_movingAverage.average() >= m_squelchLevel) - m_squelchState = m_sampleRate / 50; + demod /= M_PI; - if(m_squelchState > 0) { - m_squelchState--; - Complex d = ci * conj(m_lastSample); - m_lastSample = ci; - Real demod = atan2(d.imag(), d.real()) / M_PI; - demod = m_lowpass.filter(demod); - demod *= m_volume; - qint16 sample = demod * 32767; + demod = m_lowpass.filter(demod); + + if(demod < -1) + demod = -1; + else if(demod > 1) + demod = 1; + + demod *= m_running.m_volume; + sample = demod * 32700; + + } else { + sample = 0; + qDebug("!!!"); + } m_audioBuffer[m_audioBufferFill].l = sample; m_audioBuffer[m_audioBufferFill].r = sample; ++m_audioBufferFill; if(m_audioBufferFill >= m_audioBuffer.size()) { uint res = m_audioFifo->write((const quint8*)&m_audioBuffer[0], m_audioBufferFill, 1); - /* if(res != m_audioBufferFill) - qDebug("lost %u samples", m_audioBufferFill - res); - */ + qDebug("lost %u audio samples", m_audioBufferFill - res); m_audioBufferFill = 0; } - } - m_sampleDistanceRemain += (Real)m_sampleRate / 44100.0; + m_interpolatorDistanceRemain += m_interpolatorDistance; + } } } - if(m_audioFifo->write((const quint8*)&m_audioBuffer[0], m_audioBufferFill, 0) != m_audioBufferFill) - ;//qDebug("lost samples"); - m_audioBufferFill = 0; + if(m_audioBufferFill > 0) { + uint res = m_audioFifo->write((const quint8*)&m_audioBuffer[0], m_audioBufferFill, 1); + if(res != m_audioBufferFill) + qDebug("lost %u samples", m_audioBufferFill - res); + m_audioBufferFill = 0; + } if(m_sampleSink != NULL) m_sampleSink->feed(m_sampleBuffer.begin(), m_sampleBuffer.end(), firstOfBurst); @@ -111,6 +203,12 @@ void NFMDemod::feed(SampleVector::const_iterator begin, SampleVector::const_iter void NFMDemod::start() { m_squelchState = 0; + m_audioFifo->clear(); + m_audioFifo->setStopped(true); + m_interpolatorRegulation = 0.9999; + m_interpolatorDistance = 1.0; + m_interpolatorDistanceRemain = 0.0; + m_lastSample = 0; } void NFMDemod::stop() @@ -120,24 +218,20 @@ void NFMDemod::stop() bool NFMDemod::handleMessage(Message* cmd) { if(DSPSignalNotification::match(cmd)) { - DSPSignalNotification* signal = (DSPSignalNotification*)cmd; - qDebug("%d samples/sec, %lld Hz offset", signal->getSampleRate(), signal->getFrequencyOffset()); - m_sampleRate = signal->getSampleRate(); - m_nco.setFreq(-signal->getFrequencyOffset(), m_sampleRate); - m_interpolator.create(16, m_sampleRate, m_rfBandwidth / 2.1); - m_sampleDistanceRemain = m_sampleRate / 44100.0; - m_squelchState = 0; + DSPSignalNotification* signal = DSPSignalNotification::cast(cmd); + + m_config.m_inputSampleRate = signal->getSampleRate(); + m_config.m_inputFrequencyOffset = signal->getFrequencyOffset(); + apply(); cmd->completed(); return true; } else if(MsgConfigureNFMDemod::match(cmd)) { - MsgConfigureNFMDemod* cfg = (MsgConfigureNFMDemod*)cmd; - m_rfBandwidth = cfg->getRFBandwidth(); - m_interpolator.create(16, m_sampleRate, m_rfBandwidth / 2.1); - m_lowpass.create(21, 44100, cfg->getAFBandwidth()); - m_squelchLevel = pow(10.0, cfg->getSquelch() / 20.0); - m_squelchLevel *= m_squelchLevel; - m_volume = cfg->getVolume(); - cmd->completed(); + MsgConfigureNFMDemod* cfg = MsgConfigureNFMDemod::cast(cmd); + m_config.m_rfBandwidth = cfg->getRFBandwidth(); + m_config.m_afBandwidth = cfg->getAFBandwidth(); + m_config.m_volume = cfg->getVolume(); + m_config.m_squelch = cfg->getSquelch(); + apply(); return true; } else { if(m_sampleSink != NULL) @@ -145,3 +239,36 @@ bool NFMDemod::handleMessage(Message* cmd) else return false; } } + +void NFMDemod::apply() +{ + + if((m_config.m_inputFrequencyOffset != m_running.m_inputFrequencyOffset) || + (m_config.m_inputSampleRate != m_running.m_inputSampleRate)) { + m_nco.setFreq(-m_config.m_inputFrequencyOffset, m_config.m_inputSampleRate); + } + + if((m_config.m_inputSampleRate != m_running.m_inputSampleRate) || + (m_config.m_rfBandwidth != m_running.m_rfBandwidth)) { + m_interpolator.create(16, m_config.m_inputSampleRate, m_config.m_rfBandwidth / 2.2); + m_interpolatorDistanceRemain = 0; + m_interpolatorDistance = 1.0; + } + + if((m_config.m_afBandwidth != m_running.m_afBandwidth) || + (m_config.m_audioSampleRate != m_running.m_audioSampleRate)) { + m_lowpass.create(21, m_config.m_audioSampleRate, m_config.m_afBandwidth); + } + + if(m_config.m_squelch != m_running.m_squelch) { + m_squelchLevel = pow(10.0, m_config.m_squelch / 20.0); + m_squelchLevel *= m_squelchLevel; + } + + m_running.m_inputSampleRate = m_config.m_inputSampleRate; + m_running.m_inputFrequencyOffset = m_config.m_inputFrequencyOffset; + m_running.m_rfBandwidth = m_config.m_rfBandwidth; + m_running.m_squelch = m_config.m_squelch; + m_running.m_volume = m_config.m_volume; + m_running.m_audioSampleRate = m_config.m_audioSampleRate; +} |