kopia lustrzana https://github.com/mobilinkd/tnc3-firmware
Update AFSK demodulation code to use Q15 FIR filter and new HDLC code. Tune the PLL for improved performance. Clean up IIR filter code.
Rob Riggs
rodzic
56865be23c
commit
b7791ab536
|
|
@ -1,41 +1,42 @@
|
|||
// Copyright 2017 Rob Riggs <rob@mobilinkd.com>
|
||||
// Copyright 2017-2019 Rob Riggs <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#include "AfskDemodulator.hpp"
|
||||
|
||||
namespace mobilinkd { namespace tnc { namespace afsk1200 {
|
||||
|
||||
|
||||
hdlc::IoFrame* Demodulator::operator()(float* samples, size_t len)
|
||||
hdlc::IoFrame* Demodulator::operator()(q15_t* samples, size_t len)
|
||||
{
|
||||
using namespace mobilinkd::tnc::gpio;
|
||||
|
||||
hdlc::IoFrame* result = 0;
|
||||
|
||||
float* fa = audio_filter_(samples);
|
||||
std::transform(fa, fa + audio::ADC_BUFFER_SIZE, buffer_a, agc_);
|
||||
auto levels = input_comparator_(buffer_a);
|
||||
|
||||
for (size_t i = 0; i != len; i++) {
|
||||
buffer_[i] = int16_t(fa[i]);
|
||||
}
|
||||
|
||||
bool delayed = delay_line_(levels[i]);
|
||||
float_type x = float_type(levels[i] ^ delayed) - .5f;
|
||||
float_type fc = correlator_filter_(x);
|
||||
bool bit = output_comparator_(fc);
|
||||
for (size_t i = 0; i != len; i++) {
|
||||
bool level = (buffer_[i] >= 0);
|
||||
bool delayed = delay_line_(level);
|
||||
buffer_[i] = (int16_t(level ^ delayed) << 1) - 1;
|
||||
}
|
||||
|
||||
auto* fc = lpf_filter_.filter(buffer_);
|
||||
|
||||
for (size_t i = 0; i != len; i++) {
|
||||
bool bit = fc[i] >= 0;
|
||||
auto pll = pll_(bit);
|
||||
|
||||
if (pll.sample) {
|
||||
if (locked_ != pll.locked) {
|
||||
locked_ = pll.locked;
|
||||
}
|
||||
locked_ = pll.locked;
|
||||
|
||||
// We will only ever get one frame because there are
|
||||
// not enough bits in a block for more than one.
|
||||
if (result) {
|
||||
auto tmp = hdlc_decoder_(nrzi_.decode(bit), pll.locked);
|
||||
auto tmp = hdlc_decoder_(nrzi_.decode(bit), true);
|
||||
if (tmp) hdlc::release(tmp);
|
||||
} else {
|
||||
result = hdlc_decoder_(nrzi_.decode(bit), pll.locked);
|
||||
result = hdlc_decoder_(nrzi_.decode(bit), true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,91 +1,38 @@
|
|||
// Copyright 2015 Mobilinkd LLC <rob@mobilinkd.com>
|
||||
// Copyright 2015-2019 Mobilinkd LLC <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#ifndef MOBILINKD__AFSK_DEMODULATOR_HPP_
|
||||
#define MOBILINKD__AFSK_DEMODULATOR_HPP_
|
||||
|
||||
#include <arm_math.h>
|
||||
#include "AGC.hpp"
|
||||
#include "DelayLine.hpp"
|
||||
#include "AudioInput.hpp"
|
||||
#include "DigitalPLL.hpp"
|
||||
// #include "Filter.h"
|
||||
#include "HdlcDecoder.hpp"
|
||||
#include "Hysteresis.hpp"
|
||||
#include "FirFilter.hpp"
|
||||
#include "IirFilter.hpp"
|
||||
#include "NRZI.hpp"
|
||||
#include "GPIO.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
|
||||
namespace mobilinkd { namespace tnc { namespace afsk1200 {
|
||||
|
||||
#if 0
|
||||
const float b[] = {
|
||||
4.57519926037e-06,
|
||||
2.28759963018e-05,
|
||||
4.57519926037e-05,
|
||||
4.57519926037e-05,
|
||||
2.28759963018e-05,
|
||||
4.57519926037e-06,
|
||||
};
|
||||
const size_t LPF_FILTER_LEN = 96;
|
||||
|
||||
const float a[] = {
|
||||
1.0,
|
||||
-4.41489189545,
|
||||
7.82710410154,
|
||||
-6.96306748269,
|
||||
3.10736843037,
|
||||
-0.556366747391,
|
||||
};
|
||||
const q15_t lpf_coeffs[] = {
|
||||
0, 1, 3, 5, 8, 11, 14, 17, 19, 20, 18, 14,
|
||||
7, -2, -16, -33, -53, -76, -101, -126, -151, -174, -194, -208,
|
||||
-215, -212, -199, -173, -133, -79, -10, 74, 173, 287, 413, 549,
|
||||
693, 842, 993, 1142, 1287, 1423, 1547, 1656, 1747, 1817, 1865, 1889,
|
||||
1889, 1865, 1817, 1747, 1656, 1547, 1423, 1287, 1142, 993, 842, 693,
|
||||
549, 413, 287, 173, 74, -10, -79, -133, -173, -199, -212, -215,
|
||||
-208, -194, -174, -151, -126, -101, -76, -53, -33, -16, -2, 7,
|
||||
14, 18, 20, 19, 17, 14, 11, 8, 5, 3, 1, 0,
|
||||
|
||||
// Bessel 760Hz
|
||||
const float b[] = {
|
||||
4.36034607e-06,
|
||||
2.18017304e-05,
|
||||
4.36034607e-05,
|
||||
4.36034607e-05,
|
||||
2.18017304e-05,
|
||||
4.36034607e-06,
|
||||
};
|
||||
|
||||
const float a[] = {
|
||||
1.0,
|
||||
-4.32673235,
|
||||
7.51393353,
|
||||
-6.54579279,
|
||||
2.86009139,
|
||||
-0.50136025,
|
||||
};
|
||||
#endif
|
||||
|
||||
// Bessel 1200Hz 7-pole low-pass
|
||||
const float b[] = {
|
||||
6.10481382e-07,
|
||||
4.27336967e-06,
|
||||
1.28201090e-05,
|
||||
2.13668484e-05,
|
||||
2.13668484e-05,
|
||||
1.28201090e-05,
|
||||
4.27336967e-06,
|
||||
6.10481382e-07,
|
||||
};
|
||||
|
||||
const float a[] = {
|
||||
1.0,
|
||||
-5.56209875,
|
||||
13.34528507,
|
||||
-17.89828744,
|
||||
14.48661262,
|
||||
-7.07391246,
|
||||
1.92904679,
|
||||
-0.22656769,
|
||||
};
|
||||
|
||||
typedef FirFilter<audio::ADC_BUFFER_SIZE, 9> emphasis_filter_type;
|
||||
// typedef IirFilter<audio::ADC_BUFFER_SIZE, 2> emphasis_filter_type;
|
||||
|
||||
struct Demodulator {
|
||||
|
||||
|
|
@ -100,30 +47,25 @@ struct Demodulator {
|
|||
|
||||
size_t sample_rate_;
|
||||
emphasis_filter_type& audio_filter_;
|
||||
libafsk::BaseAGC<float_type> agc_;
|
||||
libafsk::BlockHysteresis<bool, audio::ADC_BUFFER_SIZE> input_comparator_;
|
||||
libafsk::FixedDelayLine<40> delay_line_;
|
||||
IirFilter<8> correlator_filter_;
|
||||
libafsk::BaseHysteresis<float_type> output_comparator_;
|
||||
DPLL pll_;
|
||||
Q15FirFilter<audio::ADC_BUFFER_SIZE, LPF_FILTER_LEN> lpf_filter_;
|
||||
libafsk::NRZI nrzi_;
|
||||
hdlc::Decoder hdlc_decoder_;
|
||||
hdlc::NewDecoder hdlc_decoder_;
|
||||
bool locked_;
|
||||
float_type buffer_a[audio::ADC_BUFFER_SIZE];
|
||||
q15_t buffer_[audio::ADC_BUFFER_SIZE];
|
||||
|
||||
Demodulator(size_t sample_rate, emphasis_filter_type& c)
|
||||
: sample_rate_(sample_rate)
|
||||
, audio_filter_(c)
|
||||
, agc_(.01, .001, 0.3, 1000.0)
|
||||
, input_comparator_(-0.0005, 0.0005)
|
||||
, delay_line_(sample_rate, 0.000448)
|
||||
, correlator_filter_(b, a)
|
||||
, output_comparator_(-0.05, 0.05)
|
||||
, pll_(sample_rate, SYMBOL_RATE)
|
||||
, nrzi_(), hdlc_decoder_(false), locked_(false)
|
||||
{}
|
||||
{
|
||||
lpf_filter_.init(lpf_coeffs);
|
||||
}
|
||||
|
||||
hdlc::IoFrame* operator()(float* samples, size_t len);
|
||||
hdlc::IoFrame* operator()(q15_t* samples, size_t len);
|
||||
|
||||
bool locked() const {return locked_;}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
// Copyright 2018 Rob Riggs <rob@mobilinkd.com>
|
||||
// Copyright 2018-2019 Rob Riggs <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#include "AudioInput.hpp"
|
||||
|
|
@ -14,6 +14,7 @@
|
|||
#include "PortInterface.hpp"
|
||||
#include "Goertzel.h"
|
||||
#include "DCD.h"
|
||||
#include "AGC.hpp"
|
||||
|
||||
#include "arm_math.h"
|
||||
#include "stm32l4xx_hal.h"
|
||||
|
|
@ -133,260 +134,30 @@ extern "C" void startAudioInputTask(void const*) {
|
|||
namespace mobilinkd { namespace tnc { namespace audio {
|
||||
|
||||
/*
|
||||
|
||||
FIR filter designed with
|
||||
http://t-filter.appspot.com
|
||||
|
||||
sampling frequency: 26400 Hz
|
||||
|
||||
* 0 Hz - 800 Hz
|
||||
gain = 0
|
||||
desired attenuation = -40 dB
|
||||
actual attenuation = -41.713187739640446 dB
|
||||
|
||||
* 1100 Hz - 2300 Hz
|
||||
gain = 1
|
||||
desired ripple = 3 dB
|
||||
actual ripple = 1.9403554103597218 dB
|
||||
|
||||
* 2600 Hz - 13200 Hz
|
||||
gain = 0
|
||||
desired attenuation = -40 dB
|
||||
actual attenuation = -41.713187739640446 dB
|
||||
|
||||
*/
|
||||
|
||||
#define FILTER_TAP_NUM 121
|
||||
|
||||
const float taps_0dB_121[] = {
|
||||
0.00404434702588704,
|
||||
-0.0003678805989470367,
|
||||
-0.0011037474176397869,
|
||||
-0.0023718433790735397,
|
||||
-0.004074774206090812,
|
||||
-0.005873042355767296,
|
||||
-0.007185024927682025,
|
||||
-0.00733586918005499,
|
||||
-0.005849936137673611,
|
||||
-0.0026340821242355635,
|
||||
0.001829866887380395,
|
||||
0.006596559367932984,
|
||||
0.010513363703436297,
|
||||
0.012581963716759209,
|
||||
0.012278717176141912,
|
||||
0.009711068794837135,
|
||||
0.005595156551222992,
|
||||
0.0010151465722928203,
|
||||
-0.0028911100291917724,
|
||||
-0.00525632756952279,
|
||||
-0.005713225280738633,
|
||||
-0.004481173891886628,
|
||||
-0.0022979447479362165,
|
||||
-0.00020042687909196258,
|
||||
0.0007698191454281245,
|
||||
-0.00010521900913954391,
|
||||
-0.0029591195718788473,
|
||||
-0.00722329412770922,
|
||||
-0.01171525034180743,
|
||||
-0.014940096229612041,
|
||||
-0.015533747313789608,
|
||||
-0.012735965353880947,
|
||||
-0.006719733175529481,
|
||||
0.0013488096154956725,
|
||||
0.00958058819325866,
|
||||
0.015905894530456915,
|
||||
0.018729580500272548,
|
||||
0.01748781569748245,
|
||||
0.012897342590446394,
|
||||
0.006795493031300682,
|
||||
0.0015881037163025886,
|
||||
-0.0005374299552534915,
|
||||
0.0016047331197704602,
|
||||
0.007673729209166328,
|
||||
0.015724242346878387,
|
||||
0.022631203832237833,
|
||||
0.024935722640487233,
|
||||
0.01989507252801227,
|
||||
0.00645078432931977,
|
||||
-0.014172629267618218,
|
||||
-0.038442876378037664,
|
||||
-0.06113274585379875,
|
||||
-0.07647113797360244,
|
||||
-0.07957164384983365,
|
||||
-0.06778356575105521,
|
||||
-0.04159845430900078,
|
||||
-0.0048373906972505945,
|
||||
0.03598583922416374,
|
||||
0.0730150987796154,
|
||||
0.09880455186134979,
|
||||
0.10804223448811107,
|
||||
0.09880455186134979,
|
||||
0.0730150987796154,
|
||||
0.03598583922416374,
|
||||
-0.0048373906972505945,
|
||||
-0.04159845430900078,
|
||||
-0.06778356575105521,
|
||||
-0.07957164384983365,
|
||||
-0.07647113797360244,
|
||||
-0.06113274585379875,
|
||||
-0.038442876378037664,
|
||||
-0.014172629267618218,
|
||||
0.006450784329319771,
|
||||
0.01989507252801227,
|
||||
0.024935722640487233,
|
||||
0.022631203832237833,
|
||||
0.015724242346878387,
|
||||
0.007673729209166332,
|
||||
0.0016047331197704602,
|
||||
-0.0005374299552534915,
|
||||
0.0015881037163025886,
|
||||
0.0067954930313006805,
|
||||
0.012897342590446394,
|
||||
0.017487815697482458,
|
||||
0.01872958050027255,
|
||||
0.015905894530456915,
|
||||
0.00958058819325866,
|
||||
0.0013488096154956762,
|
||||
-0.006719733175529481,
|
||||
-0.012735965353880947,
|
||||
-0.015533747313789608,
|
||||
-0.014940096229612034,
|
||||
-0.01171525034180743,
|
||||
-0.00722329412770922,
|
||||
-0.0029591195718788473,
|
||||
-0.00010521900913954758,
|
||||
0.0007698191454281245,
|
||||
-0.00020042687909196258,
|
||||
-0.0022979447479362165,
|
||||
-0.004481173891886626,
|
||||
-0.005713225280738633,
|
||||
-0.005256327569522788,
|
||||
-0.0028911100291917724,
|
||||
0.0010151465722928203,
|
||||
0.005595156551222992,
|
||||
0.009711068794837135,
|
||||
0.012278717176141915,
|
||||
0.012581963716759209,
|
||||
0.010513363703436297,
|
||||
0.006596559367932984,
|
||||
0.001829866887380395,
|
||||
-0.0026340821242355635,
|
||||
-0.005849936137673611,
|
||||
-0.0073358691800549875,
|
||||
-0.007185024927682024,
|
||||
-0.005873042355767296,
|
||||
-0.004074774206090811,
|
||||
-0.0023718433790735397,
|
||||
-0.0011037474176397869,
|
||||
-0.00036788059894704404,
|
||||
0.00404434702588704
|
||||
};
|
||||
|
||||
/*
|
||||
|
||||
FIR filter designed with
|
||||
http://t-filter.appspot.com
|
||||
|
||||
sampling frequency: 26400 Hz
|
||||
|
||||
* 0 Hz - 600 Hz
|
||||
gain = 0
|
||||
desired attenuation = -40 dB
|
||||
actual attenuation = -41.59537969202882 dB
|
||||
|
||||
* 1100 Hz - 2300 Hz
|
||||
gain = 1
|
||||
desired ripple = 3 dB
|
||||
actual ripple = 1.9670775534013671 dB
|
||||
|
||||
* 2800 Hz - 13200 Hz
|
||||
gain = 0
|
||||
desired attenuation = -40 dB
|
||||
actual attenuation = -41.59537969202882 dB
|
||||
|
||||
*/
|
||||
|
||||
// #define FILTER_TAP_NUM 73
|
||||
|
||||
const float taps_0dB_73[] = {
|
||||
0.0010893641938196257,
|
||||
0.0029403198794405202,
|
||||
-0.0037231874681753637,
|
||||
-0.005078116094780293,
|
||||
-0.008797286521082463,
|
||||
-0.011317340935878852,
|
||||
-0.012200463385017889,
|
||||
-0.01036925371439487,
|
||||
-0.005637326405238566,
|
||||
0.0014334055832832988,
|
||||
0.009462055516437227,
|
||||
0.016585173167785613,
|
||||
0.020968649539195763,
|
||||
0.021402512805125434,
|
||||
0.017768177789191805,
|
||||
0.011189277365350641,
|
||||
0.003796773667470304,
|
||||
-0.0019035128640327481,
|
||||
-0.003853114272608765,
|
||||
-0.0012626334798333488,
|
||||
0.004945485136075468,
|
||||
0.012177421685799305,
|
||||
0.016832103112238102,
|
||||
0.01536679512873413,
|
||||
0.005573647945409955,
|
||||
-0.012436210925634471,
|
||||
-0.03581550676890827,
|
||||
-0.05935854007614169,
|
||||
-0.07666569528110105,
|
||||
-0.08180873487685453,
|
||||
-0.07107700533422828,
|
||||
-0.0442469908102239,
|
||||
-0.005044918510227134,
|
||||
0.03944927956419565,
|
||||
0.0803589200537307,
|
||||
0.10908069178917619,
|
||||
0.11940367705752576,
|
||||
0.1090806917891762,
|
||||
0.0803589200537307,
|
||||
0.03944927956419565,
|
||||
-0.005044918510227135,
|
||||
-0.0442469908102239,
|
||||
-0.07107700533422828,
|
||||
-0.08180873487685453,
|
||||
-0.07666569528110105,
|
||||
-0.05935854007614169,
|
||||
-0.03581550676890827,
|
||||
-0.012436210925634473,
|
||||
0.005573647945409955,
|
||||
0.015366795128734134,
|
||||
0.016832103112238102,
|
||||
0.012177421685799305,
|
||||
0.004945485136075468,
|
||||
-0.0012626334798333488,
|
||||
-0.003853114272608765,
|
||||
-0.001903512864032745,
|
||||
0.003796773667470304,
|
||||
0.011189277365350641,
|
||||
0.017768177789191805,
|
||||
0.021402512805125434,
|
||||
0.020968649539195763,
|
||||
0.016585173167785607,
|
||||
0.009462055516437227,
|
||||
0.0014334055832832988,
|
||||
-0.005637326405238568,
|
||||
-0.01036925371439487,
|
||||
-0.012200463385017889,
|
||||
-0.011317340935878852,
|
||||
-0.008797286521082463,
|
||||
-0.005078116094780293,
|
||||
-0.0037231874681753637,
|
||||
0.0029403198794405202,
|
||||
0.0010893641938196242
|
||||
* Generated with Scipy Filter, 140 coefficients, 1100-2300Hz bandpass,
|
||||
* Hann window, starting and ending 0 value coefficients removed.
|
||||
*
|
||||
* firwin(140, [1100.0/(sample_rate/2), 2300.0/(sample_rate/2)], width = None,
|
||||
* pass_zero = False, scale = True, window='hann')[6:-6]
|
||||
*/
|
||||
constexpr size_t FILTER_TAP_NUM = 128;
|
||||
const q15_t bpf_coeffs[] = {
|
||||
1, 3, 6, 7, 6, 2, -7, -19, -32, -42, -45, -38,
|
||||
-22, 0, 21, 38, 44, 38, 23, 6, -3, 2, 28, 71,
|
||||
120, 158, 168, 136, 59, -53, -181, -295, -366, -373, -310, -191,
|
||||
-46, 87, 173, 190, 140, 48, -40, -74, -10, 163, 426, 718,
|
||||
953, 1038, 900, 510, -100, -840, -1568, -2125, -2365, -2199, -1612, -682,
|
||||
438, 1547, 2433, 2924, 2924, 2433, 1547, 438, -682, -1612, -2199, -2365,
|
||||
-2125, -1568, -840, -100, 510, 900, 1038, 953, 718, 426, 163, -10,
|
||||
-74, -40, 48, 140, 190, 173, 87, -46, -191, -310, -373, -366,
|
||||
-295, -181, -53, 59, 136, 168, 158, 120, 71, 28, 2, -3,
|
||||
6, 23, 38, 44, 38, 21, 0, -22, -38, -45, -42, -32,
|
||||
-19, -7, 2, 6, 7, 6, 3, 1,
|
||||
};
|
||||
|
||||
uint32_t adc_buffer[ADC_BUFFER_SIZE]; // Two samples per element.
|
||||
|
||||
typedef FirFilter<ADC_BUFFER_SIZE, FILTER_TAP_NUM> audio_filter_type;
|
||||
typedef Q15FirFilter<ADC_BUFFER_SIZE, FILTER_TAP_NUM> audio_filter_type;
|
||||
|
||||
audio_filter_type audio_filter;
|
||||
|
||||
|
|
@ -416,12 +187,14 @@ mobilinkd::tnc::afsk1200::Demodulator& getDemod3(const TFirCoefficients<9>& f) {
|
|||
return instance;
|
||||
}
|
||||
|
||||
q15_t normalized[ADC_BUFFER_SIZE];
|
||||
mobilinkd::tnc::FeedForwardAGC<q15_t, 22 * 8, ADC_BUFFER_SIZE, 8192> agc;
|
||||
|
||||
void demodulatorTask() {
|
||||
|
||||
DEBUG("enter demodulatorTask");
|
||||
|
||||
audio_filter.init(taps_0dB_121);
|
||||
audio_filter.init(bpf_coeffs);
|
||||
|
||||
// rx_twist is 6dB for discriminator input and 0db for de-emphasized input.
|
||||
auto twist = kiss::settings().rx_twist;
|
||||
|
|
@ -448,13 +221,16 @@ void demodulatorTask() {
|
|||
if (evt.status != osEventMessage) {
|
||||
continue;
|
||||
}
|
||||
|
||||
++counter;
|
||||
|
||||
auto block = (adc_pool_type::chunk_type*) evt.value.p;
|
||||
auto samples = (int16_t*) block->buffer;
|
||||
|
||||
float* audio = audio_filter(samples);
|
||||
arm_offset_q15(samples, 0 - virtual_ground, normalized, ADC_BUFFER_SIZE);
|
||||
adcPool.deallocate(block);
|
||||
q15_t* n = agc(normalized);
|
||||
q15_t* audio = audio_filter(n);
|
||||
|
||||
#if 1
|
||||
frame = demod1(audio, ADC_BUFFER_SIZE);
|
||||
|
|
|
|||
|
|
@ -1,32 +0,0 @@
|
|||
// Copyright 2017 Rob Riggs <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#include "DigitalPLL.hpp"
|
||||
|
||||
namespace mobilinkd { namespace tnc {
|
||||
|
||||
namespace pll {
|
||||
|
||||
// Loop low-pass filter taps (64Hz Bessel)
|
||||
float loop_b[] = {
|
||||
0.144668495309,
|
||||
0.144668495309,
|
||||
};
|
||||
float loop_a[] = {
|
||||
1.0,
|
||||
-0.710663009381,
|
||||
};
|
||||
|
||||
// Lock low-pass filter taps (40Hz Bessel)
|
||||
float lock_b[] = {
|
||||
0.0951079834025,
|
||||
0.0951079834025,
|
||||
};
|
||||
float lock_a[] = {
|
||||
1.0,
|
||||
-0.809784033195,
|
||||
};
|
||||
|
||||
} // pll
|
||||
|
||||
}} // mobilinkd::tnc
|
||||
|
|
@ -3,24 +3,20 @@
|
|||
|
||||
#include "Hysteresis.hpp"
|
||||
#include "IirFilter.hpp"
|
||||
#include "FirFilter.hpp"
|
||||
|
||||
#include "arm_math.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
namespace mobilinkd { namespace tnc {
|
||||
|
||||
namespace pll {
|
||||
|
||||
// Loop low-pass filter taps (64Hz Bessel)
|
||||
extern float loop_b[2];
|
||||
extern float loop_a[2];
|
||||
// Lock low-pass filter taps (40Hz Bessel)
|
||||
extern float lock_b[2];
|
||||
extern float lock_a[2];
|
||||
|
||||
template <typename FloatType>
|
||||
struct PLLResult {
|
||||
|
|
@ -29,13 +25,32 @@ struct PLLResult {
|
|||
bool locked;
|
||||
};
|
||||
|
||||
// Lock low-pass filter taps (80Hz Bessel)
|
||||
// scipy.signal:
|
||||
// b, a = bessel(4, [80.0/(1200/2)], 'lowpass')
|
||||
//
|
||||
const std::array<float, 5> lock_b = {
|
||||
1.077063e-03,4.308253e-03,6.462379e-03,4.308253e-03,1.077063e-03,
|
||||
};
|
||||
const std::array<float, 5> lock_a = {
|
||||
1.000000e+00,-2.774567e+00,2.962960e+00,-1.437990e+00,2.668296e-01,
|
||||
};
|
||||
|
||||
// 64 Hz loop filter.
|
||||
// scipy.signal:
|
||||
// loop_coeffs = firwin(9, [64.0/(1200/2)], width = None,
|
||||
// pass_zero = True, scale = True, window='hann')
|
||||
//
|
||||
const std::array<float, 7> loop_coeffs = {
|
||||
// 0.08160962754214955, 0.25029850550446403, 0.3361837339067726, 0.2502985055044641, 0.08160962754214969
|
||||
3.196252e-02,1.204223e-01,2.176819e-01,2.598666e-01,2.176819e-01,1.204223e-01,3.196252e-02
|
||||
};
|
||||
|
||||
} // pll
|
||||
|
||||
template <typename T>
|
||||
struct BaseDigitalPLL {
|
||||
|
||||
static const size_t N = 16;
|
||||
|
||||
struct BaseDigitalPLL
|
||||
{
|
||||
typedef T float_type;
|
||||
typedef pll::PLLResult<float_type> result_type;
|
||||
|
||||
|
|
@ -44,9 +59,9 @@ struct BaseDigitalPLL {
|
|||
float_type sps_; ///< Samples per symbol
|
||||
float_type limit_; ///< Samples per symbol / 2
|
||||
libafsk::BaseHysteresis<float_type> lock_;
|
||||
IirFilter<2> loop_filter_;
|
||||
IirFilter<2> lock_filter_;
|
||||
|
||||
FirFilter<1, 7> loop_filter_{pll::loop_coeffs.begin()};
|
||||
IirFilter<5> lock_filter_{pll::lock_b, pll::lock_a};
|
||||
|
||||
bool last_;
|
||||
float_type count_;
|
||||
|
||||
|
|
@ -57,20 +72,17 @@ struct BaseDigitalPLL {
|
|||
BaseDigitalPLL(float_type sample_rate, float_type symbol_rate)
|
||||
: sample_rate_(sample_rate), symbol_rate_(symbol_rate)
|
||||
, sps_(sample_rate / symbol_rate)
|
||||
, limit_(sps_ / 2.0f)
|
||||
, lock_(sps_ * 0.025, sps_ * .15, 1, 0)
|
||||
, loop_filter_(pll::loop_b, pll::loop_a)
|
||||
, lock_filter_(pll::lock_b, pll::lock_a)
|
||||
, limit_(sps_ / float_type(2.0))
|
||||
, lock_(sps_ * float_type(0.03), sps_ * float_type(0.15), 1, 0)
|
||||
, last_(false), count_(0), sample_(false)
|
||||
, jitter_(0.0), bits_(1)
|
||||
{}
|
||||
|
||||
result_type operator()(bool input)
|
||||
{
|
||||
|
||||
sample_ = false;
|
||||
|
||||
if (input != last_ or bits_ > 127) {
|
||||
if (input != last_ or bits_ > 16) {
|
||||
// Record transition.
|
||||
last_ = input;
|
||||
|
||||
|
|
@ -78,11 +90,12 @@ struct BaseDigitalPLL {
|
|||
count_ -= sps_;
|
||||
}
|
||||
|
||||
float_type offset = count_ / bits_;
|
||||
float_type jitter = loop_filter_(offset);
|
||||
jitter_ = lock_filter_(abs(offset));
|
||||
const float_type offset = count_ / bits_;
|
||||
const float_type jitter = loop_filter_(offset);
|
||||
const float_type abs_offset = std::abs(offset);
|
||||
jitter_ = lock_filter_(abs_offset);
|
||||
|
||||
count_ -= jitter * sps_ * 0.023f;
|
||||
count_ -= jitter / 2;
|
||||
|
||||
bits_ = 1;
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -4,69 +4,31 @@
|
|||
#ifndef MOBILINKD__TNC__IIR_FILTER_H_
|
||||
#define MOBILINKD__TNC__IIR_FILTER_H_
|
||||
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <cstddef>
|
||||
|
||||
namespace mobilinkd { namespace tnc {
|
||||
|
||||
template <size_t N>
|
||||
struct TIirCoefficients {
|
||||
float b[N];
|
||||
float a[N];
|
||||
};
|
||||
|
||||
struct IirCoefficients {
|
||||
size_t size;
|
||||
const float* b;
|
||||
const float* a;
|
||||
|
||||
template <size_t N>
|
||||
IirCoefficients(const TIirCoefficients<N>& c)
|
||||
: size(N), b(c.b), a(c.b)
|
||||
{}
|
||||
};
|
||||
|
||||
template <size_t N>
|
||||
struct IirFilter {
|
||||
|
||||
typedef float float_type;
|
||||
const float* numerator_;
|
||||
const float* denominator_;
|
||||
float history_[N];
|
||||
const std::array<float, N>& numerator_;
|
||||
const std::array<float, N>& denominator_;
|
||||
std::array<float, N> history_;
|
||||
size_t size_;
|
||||
|
||||
IirFilter(const float* b, const float* a)
|
||||
IirFilter(const std::array<float, N>& b,
|
||||
const std::array<float, N>& a)
|
||||
: numerator_(b), denominator_(a)
|
||||
, history_(), size_(N)
|
||||
{
|
||||
memset(history_, 0, N);
|
||||
}
|
||||
|
||||
IirFilter(const float* b, const float* a, size_t size)
|
||||
: numerator_(b), denominator_(a)
|
||||
, history_(), size_(size)
|
||||
{
|
||||
memset(history_, 0, N);
|
||||
}
|
||||
|
||||
template <size_t M>
|
||||
IirFilter(TIirCoefficients<M> c)
|
||||
: numerator_(c.b), denominator_(c.a)
|
||||
, history_(), size_(M)
|
||||
{
|
||||
memset(history_, 0, N);
|
||||
}
|
||||
|
||||
IirFilter(IirCoefficients c)
|
||||
: numerator_(c.b), denominator_(c.a)
|
||||
, history_(), size_(c.size)
|
||||
{
|
||||
memset(history_, 0, N);
|
||||
history_.fill(0.0f);
|
||||
}
|
||||
|
||||
~IirFilter() {}
|
||||
|
||||
float_type operator()(float_type input)
|
||||
float operator()(float input)
|
||||
{
|
||||
|
||||
for(size_t i = size_ - 1; i != 0; i--) history_[i] = history_[i - 1];
|
||||
|
|
@ -77,7 +39,7 @@ struct IirFilter {
|
|||
history_[0] -= denominator_[i] * history_[i];
|
||||
}
|
||||
|
||||
float_type result = 0;
|
||||
float result = 0;
|
||||
for (size_t i = 0; i != size_; i++) {
|
||||
result += numerator_[i] * history_[i];
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue