From 3548060ae06bedd48f6e9034c4626da066f5e09d Mon Sep 17 00:00:00 2001 From: Rob Riggs Date: Sun, 20 Jun 2021 20:37:40 -0500 Subject: [PATCH] Add new components for updated M17 demodulator. --- TNC/ClockRecovery.h | 241 ++++++++++++++++++++++++++++++++++++++++ TNC/Correlator.cpp | 63 +++++++++++ TNC/Correlator.h | 170 ++++++++++++++++++++++++++++ TNC/DataCarrierDetect.h | 76 +++++++++++++ TNC/FreqDevEstimator.h | 129 +++++++++++++++++++++ TNC/SlidingDFT.h | 132 ++++++++++++++++++++++ 6 files changed, 811 insertions(+) create mode 100644 TNC/ClockRecovery.h create mode 100644 TNC/Correlator.cpp create mode 100644 TNC/Correlator.h create mode 100644 TNC/DataCarrierDetect.h create mode 100644 TNC/FreqDevEstimator.h create mode 100644 TNC/SlidingDFT.h diff --git a/TNC/ClockRecovery.h b/TNC/ClockRecovery.h new file mode 100644 index 0000000..d2d1803 --- /dev/null +++ b/TNC/ClockRecovery.h @@ -0,0 +1,241 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include +#include +#include +#include + +namespace mobilinkd { namespace m17 { + +/** + * Calculate the phase estimates for each sample position. + * + * This performs a running calculation of the phase of each bit position. + * It is very noisy for individual samples, but quite accurate when + * averaged over an entire M17 frame. + * + * It is designed to be used to calculate the best bit position for each + * frame of data. Samples are collected and averaged. When update() is + * called, the best sample index and clock are estimated, and the counters + * reset for the next frame. + * + * It starts counting bit 0 as the first bit received after a reset. + * + * This is very efficient as it only uses addition and subtraction for + * each bit sample. And uses one multiply and divide per update (per + * frame). + * + * This will permit a clock error of up to 500ppm. This allows up to + * 250ppm error for both transmitter and receiver clocks. This is + * less than one sample per frame when the sample rate is 48000 SPS. + * + * @inv current_index_ is in the interval [0, SAMPLES_PER_SYMBOL). + * @inv sample_index_ is in the interval [0, SAMPLES_PER_SYMBOL). + * @inv clock_ is in the interval [0.9995, 1.0005] + */ +template +class ClockRecovery +{ + static constexpr size_t SAMPLES_PER_SYMBOL = SampleRate / SymbolRate; + static constexpr int8_t MAX_OFFSET = SAMPLES_PER_SYMBOL / 2; + static constexpr FloatType dx = 1.0 / SAMPLES_PER_SYMBOL; + static constexpr FloatType MAX_CLOCK = 1.0005; + static constexpr FloatType MIN_CLOCK = 0.9995; + + std::array estimates_; + size_t sample_count_ = 0; + uint16_t frame_count_ = 0; + uint8_t sample_index_ = 0; + uint8_t prev_sample_index_ = 0; + uint8_t index_ = 0; + FloatType offset_ = 0.0; + FloatType clock_ = 1.0; + FloatType prev_sample_ = 0.0; + + /** + * Find the sample index. + * + * There are @p SAMPLES_PER_INDEX bins. It is expected that half are + * positive values and half are negative. The positive and negative + * bins will be grouped together such that there is a single transition + * from positive values to negative values. + * + * The best bit position is always the position with the positive value + * at that transition point. It will be the bit index with the highest + * energy. + * + * @post sample_index_ contains the best sample point. + */ + void update_sample_index_() + { + uint8_t index = 0; + + // Find falling edge. + bool is_positive = false; + for (size_t i = 0; i != SAMPLES_PER_SYMBOL; ++i) + { + FloatType phase = estimates_[i]; + + if (!is_positive && phase > 0) + { + is_positive = true; + } + else if (is_positive && phase < 0) + { + index = i; + break; + } + } + + sample_index_ = index == 0 ? SAMPLES_PER_SYMBOL - 1 : index - 1; + } + + /** + * Compute the drift in sample points from the last update. + * + * This should never be greater than one. + */ + FloatType calc_offset_() + { + int8_t offset = sample_index_ - prev_sample_index_; + + // When in spec, the clock should drift by less than 1 sample per frame. + if (__builtin_expect(offset >= MAX_OFFSET, 0)) + { + offset -= SAMPLES_PER_SYMBOL; + } + else if (__builtin_expect(offset <= -MAX_OFFSET, 0)) + { + offset += SAMPLES_PER_SYMBOL; + } + + return offset; + } + + void update_clock_() + { + // update_sample_index_() must be called first. + + if (__builtin_expect((frame_count_ == 0), 0)) + { + prev_sample_index_ = sample_index_; + offset_ = 0.0; + clock_ = 1.0; + return; + } + + offset_ += calc_offset_(); + prev_sample_index_ = sample_index_; + clock_ = 1.0 + (offset_ / (frame_count_ * sample_count_)); + clock_ = std::min(MAX_CLOCK, std::max(MIN_CLOCK, clock_)); + } + +public: + ClockRecovery() + { + estimates_.fill(0); + } + + /** + * Update clock recovery with the given sample. This will advance the + * current sample index by 1. + */ + void operator()(FloatType sample) + { + FloatType dy = (sample - prev_sample_); + + if (sample + prev_sample_ < 0) + { + // Invert the phase estimate when sample midpoint is less than 0. + dy = -dy; + } + + prev_sample_ = sample; + + estimates_[index_] += dy; + index_ += 1; + if (index_ == SAMPLES_PER_SYMBOL) + { + index_ = 0; + } + sample_count_ += 1; + } + + /** + * Reset the state of the clock recovery system. This should be called + * when a new transmission is detected. + */ + void reset() + { + sample_count_ = 0; + frame_count_ = 0; + index_ = 0; + sample_index_ = 0; + estimates_.fill(0); + clock_ = 1.0; + } + + /** + * Return the current sample index. This will always be in the range of + * [0..SAMPLES_PER_SYMBOL). + */ + uint8_t current_index() const + { + return index_; + } + + /** + * Return the estimated sample clock increment based on the last update. + * + * The value is only valid after samples have been collected and update() + * has been called. + */ + FloatType clock_estimate() const + { + return clock_; + } + + /** + * Return the estimated "best sample index" based on the last update. + * + * The value is only valid after samples have been collected and update() + * has been called. + */ + uint8_t sample_index() const + { + return sample_index_; + } + + /** + * Update the sample index and clock estimates, and reset the state for + * the next frame of data. + * + * @pre index_ = 0 + * @pre sample_count_ > 0 + * + * After this is called, sample_index() and clock_estimate() will have + * valid, updated results. + * + * The more samples between calls to update, the more accurate the + * estimates will be. + * + * @return true if the preconditions are met and the update has been + * performed, otherwise false. + */ + bool update() + { + assert(sample_count_ != 0 && index_ == 0); + + update_sample_index_(); + update_clock_(); + + frame_count_ = std::min(0x1000, 1 + frame_count_); + sample_count_ = 0; + estimates_.fill(0); + return true; + } +}; + +}} // mobilinkd::m17 diff --git a/TNC/Correlator.cpp b/TNC/Correlator.cpp new file mode 100644 index 0000000..05b3d0e --- /dev/null +++ b/TNC/Correlator.cpp @@ -0,0 +1,63 @@ +// Copyright 2021 Rob Riggs +// All rights reserved. + +#include "Correlator.h" +#include "Log.h" + +namespace mobilinkd { namespace m17 { + +void Correlator::sample(float value) +{ + limit_ = sample_filter(std::abs(value)); + buffer_[buffer_pos_] = value; + prev_buffer_pos_ = buffer_pos_; + if (++buffer_pos_ == buffer_.size()) buffer_pos_ = 0; +} + +float Correlator::correlate(sync_t sync) +{ + float result = 0.f; + size_t pos = prev_buffer_pos_ + SAMPLES_PER_SYMBOL; + + for (size_t i = 0; i != sync.size(); ++i) + { + if (pos >= buffer_.size()) pos -= buffer_.size(); // wrapped + result += sync[i] * buffer_[pos]; + pos += SAMPLES_PER_SYMBOL; + } + return result; +} + + +/** + * Get the average outer symbol levels at a given index. This makes three + * assumptions. + * + * 1. The max symbol value is above 0 and the min symbol value is below 0. + * 2. The samples at the given index only contain outer symbols. + * 3. The index is a peak correlation index. + * + * The first should hold true except for extreme frequency errors. The + * second holds true for the sync words used for M17. + */ +std::tuple Correlator::outer_symbol_levels(uint8_t sample_index) +{ + float min_sum = 0; + float max_sum = 0; + uint8_t min_count = 0; + uint8_t max_count = 0; + uint8_t index = 0; + for (size_t i = sample_index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL) + { + tmp[index++] = buffer_[i] * 1000.f; + max_sum += buffer_[i] * (buffer_[i] > 0.f); + min_sum += buffer_[i] * (buffer_[i] < 0.f); + max_count += (buffer_[i] > 0.f); + min_count += (buffer_[i] < 0.f); + } + INFO("osl: %d, %d, %d, %d,%d, %d, %d, %d", + tmp[0], tmp[1], tmp[2], tmp[3], tmp[4], tmp[5], tmp[6], tmp[7]); + return std::make_tuple(min_sum / min_count, max_sum / max_count); +} + +}} // mobilinkd::m17 diff --git a/TNC/Correlator.h b/TNC/Correlator.h new file mode 100644 index 0000000..3fca7ea --- /dev/null +++ b/TNC/Correlator.h @@ -0,0 +1,170 @@ +// Copyright 2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include "IirFilter.hpp" + +#include "stm32l4xx_hal.h" + +#include +#include +#include +#include +#include +#include + +namespace mobilinkd { namespace m17 { + +struct Correlator +{ + static constexpr size_t SYMBOLS = 8; + static constexpr size_t SAMPLES_PER_SYMBOL = 10; + + using value_type = float; + using buffer_t = std::array; + using sync_t = std::array; + using sample_filter_t = tnc::IirFilter<3>; + + buffer_t buffer_; + + float limit_ = 0.; + uint8_t symbol_pos_ = 0; + uint8_t buffer_pos_ = 0; + uint8_t prev_buffer_pos_ = 0; + int code = -1; + + // IIR with Nyquist of 1/240. This is used to determine the baseline + // signal level, which is then used to scale the correlation value. + // This makes the detector self-calibrating. + static constexpr std::array b = {4.24433681e-05, 8.48867363e-05, 4.24433681e-05}; + static constexpr std::array a = {1.0, -1.98148851, 0.98165828}; + sample_filter_t sample_filter{b, a}; + std::array tmp; + + void sample(float value); + + float correlate(sync_t sync); + + float limit() const {return limit_;} + uint8_t index() const {return prev_buffer_pos_ % SAMPLES_PER_SYMBOL;} + + /** + * Get the average outer symbol levels at a given index. This makes trhee + * assumptions. + * + * 1. The max symbol value is above 0 and the min symbol value is below 0. + * 2. The samples at the given index only contain outer symbols. + * 3. The index is a peak correlation index. + * + * The first should hold true except for extreme frequency errors. The + * second holds true for the sync words used for M17. + */ + std::tuple outer_symbol_levels(uint8_t sample_index); + + template + void apply(F func, uint8_t index) + { + for (size_t i = index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL) + { + func(buffer_[i]); + } + } +}; + +struct Indicator +{ + GPIO_TypeDef* gpio; + uint16_t pin; + + void on() + { + HAL_GPIO_WritePin(gpio, pin, GPIO_PIN_SET); + } + + void off() + { + HAL_GPIO_WritePin(gpio, pin, GPIO_PIN_RESET); + } +}; + +template +struct SyncWord +{ + static constexpr size_t SYMBOLS = Correlator::SYMBOLS; + static constexpr size_t SAMPLES_PER_SYMBOL = Correlator::SAMPLES_PER_SYMBOL; + using value_type = typename Correlator::value_type; + + using buffer_t = std::array; + using sample_buffer_t = std::array; + + buffer_t sync_word_; + sample_buffer_t samples_; + uint8_t pos_ = 0; + uint8_t timing_index_ = 0; + bool triggered_ = false; + int8_t updated_ = 0; + float magnitude_1_ = 1.f; + float magnitude_2_ = -1.f; + + SyncWord(buffer_t&& sync_word, float magnitude_1, float magnitude_2 = std::numeric_limits::lowest()) + : sync_word_(std::move(sync_word)), magnitude_1_(magnitude_1), magnitude_2_(magnitude_2) + {} + + float triggered(Correlator& correlator) + { + float limit_1 = correlator.limit() * magnitude_1_; + float limit_2 = correlator.limit() * magnitude_2_; + auto value = correlator.correlate(sync_word_); + + return (value > limit_1 || value < limit_2) ? value : 0.0; + } + + uint8_t operator()(Correlator& correlator) + { + auto value = triggered(correlator); + + value_type peak_value = 0; + + if (std::abs(value) > 0.0) + { + if (!triggered_) + { + samples_.fill(0); + triggered_ = true; + } + samples_[correlator.index()] = value; + } + else + { + if (triggered_) + { + // Calculate the timing index on the falling edge. + triggered_ = false; + timing_index_ = 0; + peak_value = value; + uint8_t index = 0; + for (auto f : samples_) + { + if (abs(f) > abs(peak_value)) + { + peak_value = f; + timing_index_ = index; + } + index += 1; + } + updated_ = peak_value > 0 ? 1 : -1; + } + } + return timing_index_; + } + + int8_t updated() + { + auto result = updated_; + updated_ = 0; + return result; + } +}; + +}} // mobilinkd::m17 diff --git a/TNC/DataCarrierDetect.h b/TNC/DataCarrierDetect.h new file mode 100644 index 0000000..f15275d --- /dev/null +++ b/TNC/DataCarrierDetect.h @@ -0,0 +1,76 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include "SlidingDFT.h" + +#include +#include +#include + +namespace mobilinkd { namespace m17 { + +/** + * Data carrier detection using the difference of two DFTs, one in-band and + * one out-of-band. The first frequency is the in-band frequency and the + * second one is the out-of-band Frequency. The second frequency must be + * within the normal passband of the receiver, but beyond the normal roll-off + * frequency of the data carrier. + * + * This version uses the NSlidingDFT implementation to reduce the memory + * footprint. + * + * As an example, the cut-off for 4.8k symbol/sec 4-FSK is 2400Hz, so 3000Hz + * is a reasonable out-of-band frequency to use. + * + * Note: the input to this DCD must be unfiltered (raw) baseband input. + */ +template +struct DataCarrierDetect +{ + using ComplexType = std::complex; + using NDFT = NSlidingDFT; + + NDFT dft_; + FloatType ltrigger_; + FloatType htrigger_; + FloatType level_1 = 0.0; + FloatType level_2 = 0.0; + FloatType level_ = 0.0; + bool triggered_ = false; + + DataCarrierDetect( + size_t freq1, size_t freq2, + FloatType ltrigger = 2.0, FloatType htrigger = 5.0) + : dft_({freq1, freq2}), ltrigger_(ltrigger), htrigger_(htrigger) + { + } + + /** + * Accept unfiltered baseband input and output a decision on whether + * a carrier has been detected after every @tparam BlockSize inputs. + */ + void operator()(FloatType sample) + { + auto result = dft_(sample); + level_1 += std::norm(result[0]); + level_2 += std::norm(result[1]); + } + + /** + * Update the data carrier detection level. + */ + void update() + { + level_ = level_ * 0.8 + 0.2 * (level_1 / level_2); + level_1 = 0.0; + level_2 = 0.0; + triggered_ = triggered_ ? level_ > ltrigger_ : level_ > htrigger_; + } + + + FloatType level() const { return level_; } + bool dcd() const { return triggered_; } +}; + +}} // mobilinkd::m17 diff --git a/TNC/FreqDevEstimator.h b/TNC/FreqDevEstimator.h new file mode 100644 index 0000000..b7c9c70 --- /dev/null +++ b/TNC/FreqDevEstimator.h @@ -0,0 +1,129 @@ +// Copyright 2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include "IirFilter.hpp" + +#include +#include +#include +#include + +namespace mobilinkd { namespace m17 { + +/** + * Deviation and zero-offset estimator. + * + * Accepts samples which are periodically used to update estimates of the + * input signal deviation and zero offset. + * + * Samples must be provided at the ideal sample point (the point with the + * peak bit energy). + * + * Estimates are expected to be updated at each sync word. But they can + * be updated more frequently, such as during the preamble. + */ +template +class FreqDevEstimator +{ + using sample_filter_t = tnc::IirFilter<3>; + + // IIR with Nyquist of 1/4. + static constexpr std::array dc_b = { 0.09763107, 0.19526215, 0.09763107 }; + static constexpr std::array dc_a = { 1. , -0.94280904, 0.33333333 }; + + static constexpr FloatType MAX_DC_ERROR = 0.2; + + FloatType min_est_ = 0.0; + FloatType max_est_ = 0.0; + FloatType min_cutoff_ = 0.0; + FloatType max_cutoff_ = 0.0; + FloatType min_var_ = 0.0; + FloatType max_var_ = 0.0; + size_t min_count_ = 1; + size_t max_count_ = 1; + FloatType deviation_ = 0.0; + FloatType offset_ = 0.0; + FloatType error_ = 0.0; + FloatType idev_ = 1.0; + sample_filter_t dc_filter_{dc_b, dc_a}; + +public: + + void reset() + { + min_est_ = 0.0; + max_est_ = 0.0; + min_var_ = 0.0; + max_var_ = 0.0; + min_count_ = 1; + max_count_ = 1; + min_cutoff_ = 0.0; + max_cutoff_ = 0.0; + } + + void sample(FloatType sample) + { + if (sample < 1.5 * min_est_) + { + min_count_ = 1; + min_est_ = sample; + min_var_ = 0.0; + min_cutoff_ = min_est_ * 0.666666; + } + else if (sample < min_cutoff_) + { + min_count_ += 1; + min_est_ += sample; + FloatType var = (min_est_ / min_count_) - sample; + min_var_ += var * var; + } + else if (sample > 1.5 * max_est_) + { + max_count_ = 1; + max_est_ = sample; + max_var_ = 0.0; + max_cutoff_ = max_est_ * 0.666666; + } + else if (sample > max_cutoff_) + { + max_count_ += 1; + max_est_ += sample; + FloatType var = (max_est_ / max_count_) - sample; + max_var_ += var * var; + } + } + + /** + * Update the estimates for deviation, offset, and EVM (error). Note + * that the estimates for error are using a sloppy implementation for + * calculating variance to reduce the memory requirements. This is + * because this is designed for embedded use. + */ + void update() + { + if (max_count_ < 2 || min_count_ < 2) return; + FloatType max_ = max_est_ / max_count_; + FloatType min_ = min_est_ / min_count_; + deviation_ = (max_ - min_) / 6.0; + if (deviation_ > 0) idev_ = 1.0 / deviation_; + offset_ = dc_filter_(std::max(std::min(max_ + min_, deviation_ * MAX_DC_ERROR), deviation_ * -MAX_DC_ERROR)); + error_ = (std::sqrt(max_var_ / (max_count_ - 1)) + std::sqrt(min_var_ / (min_count_ - 1))) * 0.5 * idev_; + min_cutoff_ = offset_ - deviation_ * 2; + max_cutoff_ = offset_ + deviation_ * 2; + max_est_ = max_; + min_est_ = min_; + max_count_ = 1; + min_count_ = 1; + max_var_ = 0.0; + min_var_ = 0.0; + } + + FloatType deviation() const { return deviation_; } + FloatType offset() const { return offset_; } + FloatType error() const { return error_; } + FloatType idev() const { return idev_; } +}; + +}} // mobilinkd::m17 diff --git a/TNC/SlidingDFT.h b/TNC/SlidingDFT.h new file mode 100644 index 0000000..9efa431 --- /dev/null +++ b/TNC/SlidingDFT.h @@ -0,0 +1,132 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include +#include +#include +#include + +namespace mobilinkd +{ + +/** + * A sliding DFT algorithm. + * + * Based on 'Understanding and Implementing the Sliding DFT' + * Eric Jacobsen, 2015-04-23 + * https://www.dsprelated.com/showarticle/776.php + */ +template +class SlidingDFT +{ + using ComplexType = std::complex; + + static constexpr size_t N = SampleRate / Accuracy; + static constexpr ComplexType j{0, 1}; + static constexpr FloatType pi2 = M_PI * 2.0; + static constexpr FloatType kth = FloatType(Frequency) / FloatType(SampleRate); + + // We'd like this to be static constexpr, but std::exp is not a constexpr. + const ComplexType coeff_ = std::exp(j * pi2 * kth); + std::array samples_; + ComplexType result_{0,0}; + size_t index_ = 0; + +public: + SlidingDFT() + { + samples_.fill(0); + } + + ComplexType operator()(FloatType sample) + { + FloatType delta = sample - samples_[index_]; + result_ = (result_ + delta) * coeff_; + samples_[index_] = sample; + + index_ += 1; + if (index_ == N) index_ = 0; + + return result_; + } +}; + +/** + * A sliding DFT algorithm. + * + * Based on 'Understanding and Implementing the Sliding DFT' + * Eric Jacobsen, 2015-04-23 + * https://www.dsprelated.com/showarticle/776.php + * + * @tparam FloatType is the floating point type to use. + * @tparam SampleRate is the sample rate of the incoming data. + * @tparam N is the length of the DFT. Frequency resolution is SampleRate / N. + * @tparam K is the number of frequencies whose DFT will be calculated. + */ +template +class NSlidingDFT +{ + using ComplexType = std::complex; + + static constexpr ComplexType j{0, 1}; + static constexpr FloatType pi2 = M_PI * 2.0; + + // We'd like this to be static constexpr, but std::exp is not a constexpr. + const std::array coeff_; + std::array samples_; + std::array result_{0,0}; + size_t index_ = 0; + size_t prev_index_ = N - 1; + + static constexpr std::array + make_coefficients(const std::array& frequencies) + { + std::array result; + for (size_t i = 0; i != K; ++i) + { + FloatType k = FloatType(frequencies[i]) / FloatType(SampleRate); + result[i] = std::exp(j * pi2 * k); + } + return result; + } + +public: + using result_type = std::array; + + /** + * Construct the DFT with an array of frequencies. These frequencies + * should be less than @tparam SampleRate / 2 and a mulitple of + * @tparam SampleRate / @tparam N. No validation is performed on + * these frequencies passed to the constructor. + */ + NSlidingDFT(const std::array& frequencies) + : coeff_(make_coefficients(frequencies)) + { + samples_.fill(0); + } + + /** + * Calculate the streaming DFT from the sample, returning an array + * of results which correspond to the frequencies passed in to the + * constructor. The result is only valid after at least N samples + * have been cycled in. + */ + result_type operator()(FloatType sample) + { + FloatType delta = sample - samples_[index_]; + + for (size_t i = 0; i != K; ++i) + { + result_[i] = (result_[i] + delta) * coeff_[i] * 0.999999; + } + samples_[index_] = sample; + + index_ += 1; + if (index_ == N) index_ = 0; + + return result_; + } +}; + +} // mobilinkd