From 278a94f29e1cbc6f979bbec39fc4114b9c871776 Mon Sep 17 00:00:00 2001 From: f4exb Date: Tue, 7 Jun 2022 03:22:18 +0200 Subject: [PATCH] M17 demod: first M17 processing implementation --- plugins/channelrx/demodm17/CMakeLists.txt | 12 + plugins/channelrx/demodm17/m17/CRC16.h | 72 ++ .../channelrx/demodm17/m17/CarrierDetect.h | 41 ++ .../channelrx/demodm17/m17/ClockRecovery.h | 242 +++++++ plugins/channelrx/demodm17/m17/Convolution.h | 26 + plugins/channelrx/demodm17/m17/Correlator.cpp | 18 + plugins/channelrx/demodm17/m17/Correlator.h | 190 ++++++ .../demodm17/m17/DataCarrierDetect.h | 76 +++ .../channelrx/demodm17/m17/DeviationError.h | 96 +++ plugins/channelrx/demodm17/m17/Filter.h | 14 + plugins/channelrx/demodm17/m17/FirFilter.h | 59 ++ .../demodm17/m17/FreqDevEstimator.cpp | 17 + .../channelrx/demodm17/m17/FreqDevEstimator.h | 129 ++++ .../channelrx/demodm17/m17/FrequencyError.h | 66 ++ plugins/channelrx/demodm17/m17/Fsk4Demod.h | 156 +++++ plugins/channelrx/demodm17/m17/Golay24.cpp | 137 ++++ plugins/channelrx/demodm17/m17/Golay24.h | 109 +++ plugins/channelrx/demodm17/m17/IirFilter.h | 52 ++ .../channelrx/demodm17/m17/LinkSetupFrame.h | 131 ++++ .../channelrx/demodm17/m17/M17Demodulator.cpp | 89 +++ .../channelrx/demodm17/m17/M17Demodulator.h | 589 ++++++++++++++++ .../channelrx/demodm17/m17/M17FrameDecoder.h | 397 +++++++++++ plugins/channelrx/demodm17/m17/M17Framer.h | 62 ++ plugins/channelrx/demodm17/m17/M17Modulator.h | 637 ++++++++++++++++++ .../channelrx/demodm17/m17/M17Randomizer.h | 79 +++ .../channelrx/demodm17/m17/M17Synchronizer.h | 36 + .../channelrx/demodm17/m17/PhaseEstimator.h | 51 ++ .../demodm17/m17/PolynomialInterleaver.h | 73 ++ plugins/channelrx/demodm17/m17/SlidingDFT.h | 135 ++++ plugins/channelrx/demodm17/m17/SymbolEvm.h | 85 +++ plugins/channelrx/demodm17/m17/Trellis.h | 138 ++++ plugins/channelrx/demodm17/m17/Util.h | 428 ++++++++++++ plugins/channelrx/demodm17/m17/Viterbi.h | 242 +++++++ plugins/channelrx/demodm17/m17/ax25_frame.h | 265 ++++++++ plugins/channelrx/demodm17/m17/queue.h | 251 +++++++ plugins/channelrx/demodm17/m17demod.cpp | 14 - plugins/channelrx/demodm17/m17demod.h | 22 + plugins/channelrx/demodm17/m17demod.ui | 24 +- plugins/channelrx/demodm17/m17demodbaseband.h | 22 + .../channelrx/demodm17/m17demodfilters.cpp | 48 ++ plugins/channelrx/demodm17/m17demodfilters.h | 66 ++ plugins/channelrx/demodm17/m17demodgui.cpp | 82 ++- plugins/channelrx/demodm17/m17demodgui.h | 6 +- plugins/channelrx/demodm17/m17demodgui.ui | 579 ++++++++++++---- .../channelrx/demodm17/m17demodprocessor.cpp | 494 ++++++++++++++ .../channelrx/demodm17/m17demodprocessor.h | 145 ++++ .../channelrx/demodm17/m17demodsettings.cpp | 3 - plugins/channelrx/demodm17/m17demodsettings.h | 1 - plugins/channelrx/demodm17/m17demodsink.cpp | 81 +-- plugins/channelrx/demodm17/m17demodsink.h | 29 + 50 files changed, 6578 insertions(+), 238 deletions(-) create mode 100644 plugins/channelrx/demodm17/m17/CRC16.h create mode 100644 plugins/channelrx/demodm17/m17/CarrierDetect.h create mode 100644 plugins/channelrx/demodm17/m17/ClockRecovery.h create mode 100644 plugins/channelrx/demodm17/m17/Convolution.h create mode 100644 plugins/channelrx/demodm17/m17/Correlator.cpp create mode 100644 plugins/channelrx/demodm17/m17/Correlator.h create mode 100644 plugins/channelrx/demodm17/m17/DataCarrierDetect.h create mode 100644 plugins/channelrx/demodm17/m17/DeviationError.h create mode 100644 plugins/channelrx/demodm17/m17/Filter.h create mode 100644 plugins/channelrx/demodm17/m17/FirFilter.h create mode 100644 plugins/channelrx/demodm17/m17/FreqDevEstimator.cpp create mode 100644 plugins/channelrx/demodm17/m17/FreqDevEstimator.h create mode 100644 plugins/channelrx/demodm17/m17/FrequencyError.h create mode 100644 plugins/channelrx/demodm17/m17/Fsk4Demod.h create mode 100644 plugins/channelrx/demodm17/m17/Golay24.cpp create mode 100644 plugins/channelrx/demodm17/m17/Golay24.h create mode 100644 plugins/channelrx/demodm17/m17/IirFilter.h create mode 100644 plugins/channelrx/demodm17/m17/LinkSetupFrame.h create mode 100644 plugins/channelrx/demodm17/m17/M17Demodulator.cpp create mode 100644 plugins/channelrx/demodm17/m17/M17Demodulator.h create mode 100644 plugins/channelrx/demodm17/m17/M17FrameDecoder.h create mode 100644 plugins/channelrx/demodm17/m17/M17Framer.h create mode 100644 plugins/channelrx/demodm17/m17/M17Modulator.h create mode 100644 plugins/channelrx/demodm17/m17/M17Randomizer.h create mode 100644 plugins/channelrx/demodm17/m17/M17Synchronizer.h create mode 100644 plugins/channelrx/demodm17/m17/PhaseEstimator.h create mode 100644 plugins/channelrx/demodm17/m17/PolynomialInterleaver.h create mode 100644 plugins/channelrx/demodm17/m17/SlidingDFT.h create mode 100644 plugins/channelrx/demodm17/m17/SymbolEvm.h create mode 100644 plugins/channelrx/demodm17/m17/Trellis.h create mode 100644 plugins/channelrx/demodm17/m17/Util.h create mode 100644 plugins/channelrx/demodm17/m17/Viterbi.h create mode 100644 plugins/channelrx/demodm17/m17/ax25_frame.h create mode 100644 plugins/channelrx/demodm17/m17/queue.h create mode 100644 plugins/channelrx/demodm17/m17demodfilters.cpp create mode 100644 plugins/channelrx/demodm17/m17demodfilters.h create mode 100644 plugins/channelrx/demodm17/m17demodprocessor.cpp create mode 100644 plugins/channelrx/demodm17/m17demodprocessor.h diff --git a/plugins/channelrx/demodm17/CMakeLists.txt b/plugins/channelrx/demodm17/CMakeLists.txt index f7e92cff0..f25518827 100644 --- a/plugins/channelrx/demodm17/CMakeLists.txt +++ b/plugins/channelrx/demodm17/CMakeLists.txt @@ -8,6 +8,12 @@ set(m17_SOURCES m17demodwebapiadapter.cpp m17demodplugin.cpp m17demodbaudrates.cpp + m17demodprocessor.cpp + m17demodfilters.cpp + m17/Golay24.cpp + m17/M17Demodulator.cpp + m17/FreqDevEstimator.cpp + m17/Correlator.cpp ) set(m17_HEADERS @@ -18,6 +24,12 @@ set(m17_HEADERS m17demodwebapiadapter.h m17demodplugin.h m17demodbaudrates.h + m17demodprocessor.h + m17demodfilters.h + m17/Golay24.h + m17/M17Demodulator.h + m17/FreqDevEstimator.h + m17/Correlator.h ) include_directories( diff --git a/plugins/channelrx/demodm17/m17/CRC16.h b/plugins/channelrx/demodm17/m17/CRC16.h new file mode 100644 index 000000000..05e3d2a54 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/CRC16.h @@ -0,0 +1,72 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +namespace mobilinkd +{ + +template +struct CRC16 +{ + static constexpr uint16_t MASK = 0xFFFF; + static constexpr uint16_t LSB = 0x0001; + static constexpr uint16_t MSB = 0x8000; + + uint16_t reg_ = Init; + + void reset() + { + reg_ = Init; + + for (size_t i = 0; i != 16; ++i) + { + auto bit = reg_ & LSB; + if (bit) reg_ ^= Poly; + reg_ >>= 1; + if (bit) reg_ |= MSB; + } + + reg_ &= MASK; + } + + void operator()(uint8_t byte) + { + reg_ = crc(byte, reg_); + } + + uint16_t crc(uint8_t byte, uint16_t reg) + { + for (size_t i = 0; i != 8; ++i) + { + auto msb = reg & MSB; + reg = ((reg << 1) & MASK) | ((byte >> (7 - i)) & LSB); + if (msb) reg ^= Poly; + } + return reg & MASK; + } + + uint16_t get() + { + auto reg = reg_; + for (size_t i = 0; i != 16; ++i) + { + auto msb = reg & MSB; + reg = ((reg << 1) & MASK); + if (msb) reg ^= Poly; + } + return reg; + } + + std::array get_bytes() + { + auto crc = get(); + std::array result{uint8_t((crc >> 8) & 0xFF), uint8_t(crc & 0xFF)}; + return result; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/CarrierDetect.h b/plugins/channelrx/demodm17/m17/CarrierDetect.h new file mode 100644 index 000000000..1988fa1ed --- /dev/null +++ b/plugins/channelrx/demodm17/m17/CarrierDetect.h @@ -0,0 +1,41 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "IirFilter.h" + +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +template +struct CarrierDetect +{ + using result_t = std::tuple; + + BaseIirFilter filter_; + FloatType lock_; + FloatType unlock_; + bool locked_ = false; + + CarrierDetect(std::array const& b, std::array const& a, FloatType lock_level, FloatType unlock_level) + : filter_(b, a), lock_(lock_level), unlock_(unlock_level) + { + } + + result_t operator()(FloatType value) + { + auto filtered = filter_(std::abs(value)); + if (locked_ && (filtered > unlock_)) locked_ = false; + else if (!locked_ && (filtered < lock_)) locked_ = true; + + return std::make_tuple(locked_, filtered); + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/ClockRecovery.h b/plugins/channelrx/demodm17/m17/ClockRecovery.h new file mode 100644 index 000000000..37529e667 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/ClockRecovery.h @@ -0,0 +1,242 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +/** + * 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 (offset >= MAX_OFFSET) [[unlikely]] + { + offset -= SAMPLES_PER_SYMBOL; + } + else if (offset <= -MAX_OFFSET) [[unlikely]] + { + offset += SAMPLES_PER_SYMBOL; + } + + return offset; + } + + void update_clock_() + { + // update_sample_index_() must be called first. + + if (frame_count_ == 0) [[unlikely]] + { + 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); + } + + /** + * 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() + { + if (!(sample_count_ != 0 && index_ == 0)) return false; + + update_sample_index_(); + update_clock_(); + + frame_count_ = std::min(0x1000, 1 + frame_count_); + sample_count_ = 0; + estimates_.fill(0); + return true; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Convolution.h b/plugins/channelrx/demodm17/m17/Convolution.h new file mode 100644 index 000000000..e8dbd4b0a --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Convolution.h @@ -0,0 +1,26 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +#include "Util.h" + +namespace mobilinkd +{ + +inline constexpr uint32_t convolve_bit(uint32_t poly, uint32_t memory) +{ + return popcount(poly & memory) & 1; +} + +template +inline constexpr uint32_t update_memory(uint32_t memory, uint32_t input) +{ + return (memory << k | input) & ((1 << (K + 1)) - 1); +} + + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Correlator.cpp b/plugins/channelrx/demodm17/m17/Correlator.cpp new file mode 100644 index 000000000..863b1b3f3 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Correlator.cpp @@ -0,0 +1,18 @@ +#include "Correlator.h" + +namespace mobilinkd { + +// IIR with Nyquist of 1/240. +template<> +const std::array Correlator::b = {4.24433681e-05, 8.48867363e-05, 4.24433681e-05}; + +template<> +const std::array Correlator::a = {1.0, -1.98148851, 0.98165828}; + +template<> +const std::array Correlator::b = {4.24433681e-05, 8.48867363e-05, 4.24433681e-05}; + +template<> +const std::array Correlator::a = {1.0, -1.98148851, 0.98165828}; + +} // namespace mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Correlator.h b/plugins/channelrx/demodm17/m17/Correlator.h new file mode 100644 index 000000000..7822ecfa6 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Correlator.h @@ -0,0 +1,190 @@ +// Copyright 2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include "IirFilter.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace mobilinkd { + +template +struct Correlator +{ + static constexpr size_t SYMBOLS = 8; + static constexpr size_t SAMPLES_PER_SYMBOL = 10; + + using value_type = FloatType; + using buffer_t = std::array; + using sync_t = std::array; + using sample_filter_t = BaseIirFilter; + + buffer_t buffer_; + + FloatType limit_ = 0.; + size_t symbol_pos_ = 0; + size_t buffer_pos_ = 0; + size_t prev_buffer_pos_ = 0; + int code = -1; + + // IIR with Nyquist of 1/240. + static const std::array b; + static const std::array a; + sample_filter_t sample_filter{b, a}; + std::array tmp; + + void sample(FloatType value) + { + limit_ = sample_filter(std::abs(value)); + buffer_[buffer_pos_] = value; + prev_buffer_pos_ = buffer_pos_; + if (++buffer_pos_ == buffer_.size()) buffer_pos_ = 0; + } + + FloatType correlate(sync_t sync) + { + FloatType result = 0.; + 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; + } + + FloatType limit() const {return limit_;} + size_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. The third will + * hold true if passed the timing index from a triggered sync word. + */ + std::tuple outer_symbol_levels(size_t sample_index) + { + FloatType min_sum = 0; + FloatType max_sum = 0; + size_t min_count = 0; + size_t max_count = 0; + size_t index = 0; + for (size_t i = sample_index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL) + { + tmp[index++] = buffer_[i] * 1000.; + max_sum += buffer_[i] * ((buffer_[i] > 0.)); + min_sum += buffer_[i] * ((buffer_[i] < 0.)); + max_count += (buffer_[i] > 0.); + min_count += (buffer_[i] < 0.); + } + + return std::make_tuple(min_sum / min_count, max_sum / max_count); + } + + + template + void apply(F func, uint8_t index) + { + for (size_t i = index; i < buffer_.size(); i += SAMPLES_PER_SYMBOL) + { + func(buffer_[i]); + } + } +}; + +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_; + size_t pos_ = 0; + size_t timing_index_ = 0; + bool triggered_ = false; + int8_t updated_ = 0; + value_type magnitude_1_ = 1.; + value_type magnitude_2_ = -1.; + + SyncWord(buffer_t&& sync_word, value_type magnitude_1, value_type magnitude_2 = std::numeric_limits::lowest()) + : sync_word_(std::move(sync_word)), magnitude_1_(magnitude_1), magnitude_2_(magnitude_2) + {} + + value_type triggered(Correlator& correlator) + { + value_type limit_1 = correlator.limit() * magnitude_1_; + value_type limit_2 = correlator.limit() * magnitude_2_; + auto value = correlator.correlate(sync_word_); + + return (value > limit_1 || value < limit_2) ? value : 0.0; + } + + size_t operator()(Correlator& correlator) + { + auto value = triggered(correlator); + + value_type peak_value = 0; + + if (value != 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 diff --git a/plugins/channelrx/demodm17/m17/DataCarrierDetect.h b/plugins/channelrx/demodm17/m17/DataCarrierDetect.h new file mode 100644 index 000000000..31a89318d --- /dev/null +++ b/plugins/channelrx/demodm17/m17/DataCarrierDetect.h @@ -0,0 +1,76 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include "SlidingDFT.h" + +#include +#include +#include + +namespace mobilinkd { + +/** + * 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 diff --git a/plugins/channelrx/demodm17/m17/DeviationError.h b/plugins/channelrx/demodm17/m17/DeviationError.h new file mode 100644 index 000000000..51c475b02 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/DeviationError.h @@ -0,0 +1,96 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +namespace mobilinkd +{ + +template +struct DeviationError +{ + using float_type = T; + using array_t = std::array; + + array_t minima_{0}; + array_t maxima_{0}; + size_t min_index_ = 0; + size_t max_index_ = 0; + bool min_rolled_ = false; + bool max_rolled_ = false; + size_t min_count_ = 0; + size_t max_count_ = 0; + float_type min_estimate_ = 0.0; + float_type max_estimate_ = 0.0; + + const float_type ZERO = 0.0; + + DeviationError() + { + minima_.fill(0.0); + maxima_.fill(0.0); + } + + float_type operator()(float_type sample) + { + if (sample > ZERO) + { + if (sample > max_estimate_ * 0.67 or max_count_ == 5) + { + max_count_ = 0; + maxima_[max_index_++] = sample; + if (max_index_ == N) + { + max_rolled_ = true; + max_index_ = 0; + } + if (max_rolled_) + { + max_estimate_ = std::accumulate(std::begin(maxima_), std::end(maxima_), ZERO) / N; + } + else + { + max_estimate_ = std::accumulate(std::begin(maxima_), std::begin(maxima_) + max_index_, ZERO) / max_index_; + } + } + else + { + ++max_count_; + } + } + else if (sample < 0) + { + if (sample < min_estimate_ * 0.67 or min_count_ == 5) + { + min_count_ = 0; + minima_[min_index_++] = sample; + if (min_index_ == N) + { + min_rolled_ = true; + min_index_ = 0; + } + if (min_rolled_) + { + min_estimate_ = std::accumulate(std::begin(minima_), std::end(minima_), ZERO) / N; + } + else + { + min_estimate_ = std::accumulate(std::begin(minima_), std::begin(minima_) + min_index_, ZERO) / min_index_; + } + } + else + { + ++min_count_; + } + } + + auto deviation = max_estimate_ - min_estimate_; + auto deviation_error = std::min(6.0 / deviation, 100.0); + return deviation_error; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Filter.h b/plugins/channelrx/demodm17/m17/Filter.h new file mode 100644 index 000000000..92f3a4e8d --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Filter.h @@ -0,0 +1,14 @@ +// Copyright 2015-2021 Mobilinkd LLC. + +#pragma once + +namespace mobilinkd +{ + +template +struct FilterBase +{ + virtual NumericType operator()(NumericType input) = 0; +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/FirFilter.h b/plugins/channelrx/demodm17/m17/FirFilter.h new file mode 100644 index 000000000..93953b3b5 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/FirFilter.h @@ -0,0 +1,59 @@ +// Copyright 2015-2020 Mobilinkd LLC. + +#pragma once + +#include "Filter.h" + +#include +#include + +namespace mobilinkd +{ + +template +struct BaseFirFilter : FilterBase +{ + using array_t = std::array; + + const array_t& taps_; + array_t history_; + size_t pos_ = 0; + + BaseFirFilter(const array_t& taps) + : taps_(taps) + { + history_.fill(0.0); + } + + FloatType operator()(FloatType input) override + { + history_[pos_++] = input; + if (pos_ == N) pos_ = 0; + + FloatType result = 0.0; + size_t index = pos_; + + for (size_t i = 0; i != N; ++i) + { + index = (index != 0 ? index - 1 : N - 1); + result += history_.at(index) * taps_.at(i); + } + + return result; + } + + void reset() + { + history_.fill(0.0); + pos_ = 0; + } +}; + +template +BaseFirFilter makeFirFilter(const std::array& taps) +{ + return std::move(BaseFirFilter(taps)); +} + + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/FreqDevEstimator.cpp b/plugins/channelrx/demodm17/m17/FreqDevEstimator.cpp new file mode 100644 index 000000000..0cb0ecb1f --- /dev/null +++ b/plugins/channelrx/demodm17/m17/FreqDevEstimator.cpp @@ -0,0 +1,17 @@ +#include "FreqDevEstimator.h" + +namespace mobilinkd { + +template<> +const std::array FreqDevEstimator::dc_b = { 0.09763107, 0.19526215, 0.09763107 }; + +template<> +const std::array FreqDevEstimator::dc_a = { 1. , -0.94280904, 0.33333333 }; + +template<> +const std::array FreqDevEstimator::dc_b = { 0.09763107, 0.19526215, 0.09763107 }; + +template<> +const std::array FreqDevEstimator::dc_a = { 1. , -0.94280904, 0.33333333 }; + +} // namespace mobilinkd diff --git a/plugins/channelrx/demodm17/m17/FreqDevEstimator.h b/plugins/channelrx/demodm17/m17/FreqDevEstimator.h new file mode 100644 index 000000000..9f8d49fa7 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/FreqDevEstimator.h @@ -0,0 +1,129 @@ +// Copyright 2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include "IirFilter.h" + +#include +#include +#include +#include + +namespace mobilinkd { + +/** + * 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 = BaseIirFilter; + + // IIR with Nyquist of 1/4. + static const std::array dc_b; + static const std::array dc_a; + + 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_ = 0; + size_t max_count_ = 0; + 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_ = 0; + max_count_ = 0; + 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; + 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; + if (deviation_ > 0) idev_ = 1.0 / deviation_; + 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 diff --git a/plugins/channelrx/demodm17/m17/FrequencyError.h b/plugins/channelrx/demodm17/m17/FrequencyError.h new file mode 100644 index 000000000..3534ade8e --- /dev/null +++ b/plugins/channelrx/demodm17/m17/FrequencyError.h @@ -0,0 +1,66 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "IirFilter.h" + +#include +#include +#include + +namespace mobilinkd +{ + +template +struct FrequencyError +{ + using float_type = FloatType; + using array_t = std::array; + using filter_type = BaseIirFilter; + + static constexpr std::array evm_b{0.02008337, 0.04016673, 0.02008337}; + static constexpr std::array evm_a{1.0, -1.56101808, 0.64135154}; + + array_t samples_{0}; + size_t index_ = 0; + float_type accum_ = 0.0; + filter_type filter_{makeIirFilter(evm_b, evm_a)}; + + + const float_type ZERO = 0.0; + + FrequencyError() + { + samples_.fill(0.0); + } + + auto operator()(float_type sample) + { + FloatType evm = 0; + bool use = true; + + if (sample > 2) + { + evm = sample - 3; + } + else if (sample >= -2) + { + use = false; + } + else + { + evm = sample + 3; + } + + if (use) + { + accum_ = accum_ - samples_[index_] + evm; + samples_[index_++] = evm; + if (index_ == N) index_ = 0; + } + + return filter_(accum_ / N); + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Fsk4Demod.h b/plugins/channelrx/demodm17/m17/Fsk4Demod.h new file mode 100644 index 000000000..33fd42e3c --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Fsk4Demod.h @@ -0,0 +1,156 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "FirFilter.h" +#include "PhaseEstimator.h" +#include "DeviationError.h" +#include "FrequencyError.h" +#include "SymbolEvm.h" + +#include +#include + +namespace mobilinkd +{ + +namespace detail +{ +static const auto rrc_taps = std::array{ + -0.009265784007800534, -0.006136551625729697, -0.001125978562075172, 0.004891777252042491, + 0.01071805138282269, 0.01505751553351295, 0.01679337935001369, 0.015256245142156299, + 0.01042830577908502, 0.003031522725559901, -0.0055333532968188165, -0.013403099825723372, + -0.018598682349642525, -0.01944761739590459, -0.015005271935951746, -0.0053887880354343935, + 0.008056525910253532, 0.022816244158307273, 0.035513467692208076, 0.04244131815783876, + 0.04025481153629372, 0.02671818654865632, 0.0013810216516704976, -0.03394615682795165, + -0.07502635967975885, -0.11540977897637611, -0.14703962203941534, -0.16119995609538576, + -0.14969512896336504, -0.10610329539459686, -0.026921412469634916, 0.08757875030779196, + 0.23293327870303457, 0.4006012210123992, 0.5786324696325503, 0.7528286479934068, + 0.908262741447522, 1.0309661131633199, 1.1095611856548013, 1.1366197723675815, + 1.1095611856548013, 1.0309661131633199, 0.908262741447522, 0.7528286479934068, + 0.5786324696325503, 0.4006012210123992, 0.23293327870303457, 0.08757875030779196, + -0.026921412469634916, -0.10610329539459686, -0.14969512896336504, -0.16119995609538576, + -0.14703962203941534, -0.11540977897637611, -0.07502635967975885, -0.03394615682795165, + 0.0013810216516704976, 0.02671818654865632, 0.04025481153629372, 0.04244131815783876, + 0.035513467692208076, 0.022816244158307273, 0.008056525910253532, -0.0053887880354343935, + -0.015005271935951746, -0.01944761739590459, -0.018598682349642525, -0.013403099825723372, + -0.0055333532968188165, 0.003031522725559901, 0.01042830577908502, 0.015256245142156299, + 0.01679337935001369, 0.01505751553351295, 0.01071805138282269, 0.004891777252042491, + -0.001125978562075172, -0.006136551625729697, -0.009265784007800534 +}; + +static const auto evm_b = std::array{0.02008337, 0.04016673, 0.02008337}; +static const auto evm_a = std::array{1.0, -1.56101808, 0.64135154}; +} // detail + +struct Fsk4Demod +{ + using demod_result_t = std::tuple; + using result_t = std::tuple; + + BaseFirFilter::value> rrc = makeFirFilter(detail::rrc_taps); + PhaseEstimator phase = PhaseEstimator(48000, 4800); + DeviationError deviation; + FrequencyError frequency; + SymbolEvm::value> symbol_evm = makeSymbolEvm(makeIirFilter(detail::evm_b, detail::evm_a)); + + double sample_rate = 48000; + double symbol_rate = 4800; + double unlock_gain = 0.02; + double lock_gain = 0.001; + std::array samples{0}; + double t = 0; + double dt = symbol_rate / sample_rate; + double ideal_dt = dt; + bool sample_now = false; + double estimated_deviation = 1.0; + double estimated_frequency_offset = 0.0; + double evm_average = 0.0; + + Fsk4Demod( + double sample_rate, + double symbol_rate, + double unlock_gain = 0.02, + double lock_gain = 0.001 + ) : + sample_rate(sample_rate), + symbol_rate(symbol_rate), + unlock_gain(unlock_gain * symbol_rate / sample_rate), + lock_gain(lock_gain * symbol_rate / sample_rate), + dt(symbol_rate / sample_rate), + ideal_dt(dt) + { + samples.fill(0.0); + } + + demod_result_t demod(bool lock) + { + estimated_deviation = deviation(samples[1]); + for (auto& sample : samples) sample *= estimated_deviation; + + estimated_frequency_offset = frequency(samples[1]); + for (auto& sample : samples) sample -= estimated_frequency_offset; + + auto phase_estimate = phase(samples); + if (samples[1] < 0) phase_estimate *= -1; + + dt = ideal_dt - (phase_estimate * (lock ? lock_gain : unlock_gain)); + t += dt; + + std::tuple evm_result = symbol_evm(samples[1]); + int symbol; + float evm; + std::tie(symbol, evm) = symbol_evm(samples[1]); + evm_average = symbol_evm.evm(); + samples[0] = samples[2]; + + return std::make_tuple(samples[1], phase_estimate, symbol, evm); + } + + /** + * Process the sample. If a symbol is ready, return a tuple + * containing the sample used, the estimated phase, the decoded + * symbol, the EVM, the deviation error and the frequency error + * (sample, phase, symbol, evm, ed, ef), otherwise None. + */ + result_t operator()(double sample, bool lock) + { + auto filtered_sample = rrc(sample); + + if (sample_now) + { + samples[2] = filtered_sample; + sample_now = false; + double prev_sample; + double phase_estimate; + int symbol; + double evm; + std::tie(prev_sample, phase_estimate, symbol, evm) = demod(lock); + return std::make_tuple( + prev_sample, + phase_estimate, + symbol, + evm, + estimated_deviation, + estimated_frequency_offset, + evm_average + ); + } + + t += dt; + if (t < 1.0) + { + samples[0] = filtered_sample; + } + else + { + t -= 1.0; + samples[1] = filtered_sample; + sample_now = true; + } + + return result_t{0, 0, 0, 0, 0, 0, 0}; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Golay24.cpp b/plugins/channelrx/demodm17/m17/Golay24.cpp new file mode 100644 index 000000000..846ecc297 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Golay24.cpp @@ -0,0 +1,137 @@ +#include "Util.h" +#include "Golay24.h" + +namespace mobilinkd { + +std::array Golay24::LUT = Golay24::make_lut(); + +Golay24::Golay24() +{} + +uint32_t Golay24::syndrome(uint32_t codeword) +{ + codeword &= 0xffffffl; + + for (size_t i = 0; i != 12; ++i) + { + if (codeword & 1) { + codeword ^= POLY; + } + + codeword >>= 1; + } + + return (codeword << 12); +} + +bool Golay24::parity(uint32_t codeword) +{ + return popcount(codeword) & 1; +} + +Golay24::SyndromeMapEntry Golay24::makeSyndromeMapEntry(uint64_t val) +{ + return SyndromeMapEntry{uint32_t(val >> 16), uint16_t(val & 0xFFFF)}; +} + +uint64_t Golay24::makeSME(uint64_t syndrome, uint32_t bits) +{ + return (syndrome << 24) | (bits & 0xFFFFFF); +} + +uint32_t Golay24::encode23(uint16_t data) +{ + // data &= 0xfff; + uint32_t codeword = data; + + for (size_t i = 0; i != 12; ++i) + { + if (codeword & 1) { + codeword ^= POLY; + } + + codeword >>= 1; + } + + return codeword | (data << 11); +} + +uint32_t Golay24::encode24(uint16_t data) +{ + auto codeword = encode23(data); + return ((codeword << 1) | parity(codeword)); +} + +bool Golay24::decode(uint32_t input, uint32_t& output) +{ + auto syndrm = syndrome(input >> 1); + + auto it = std::lower_bound( + LUT.begin(), + LUT.end(), + syndrm, + [](const SyndromeMapEntry& sme, uint32_t val){ + return (sme.a >> 8) < val; + } + ); + + if ((it->a >> 8) == syndrm) + { + // Build the correction from the compressed entry. + auto correction = ((((it->a & 0xFF) << 16) | it->b) << 1); + // Apply the correction to the input. + output = input ^ correction; + // Only test parity for 3-bit errors. + return popcount(syndrm) < 3 || !parity(output); + } + + return false; +} + +std::array Golay24::make_lut() +{ + constexpr size_t VECLEN=23; + Golay24_detail::array result{}; + + size_t index = 0; + result[index++] = makeSME(syndrome(0), 0); + + for (size_t i = 0; i != VECLEN; ++i) + { + auto v = (1 << i); + result[index++] = makeSME(syndrome(v), v); + } + + for (size_t i = 0; i != VECLEN - 1; ++i) + { + for (size_t j = i + 1; j != VECLEN; ++j) + { + auto v = (1 << i) | (1 << j); + result[index++] = makeSME(syndrome(v), v); + } + } + + for (size_t i = 0; i != VECLEN - 2; ++i) + { + for (size_t j = i + 1; j != VECLEN - 1; ++j) + { + for (size_t k = j + 1; k != VECLEN; ++k) + { + auto v = (1 << i) | (1 << j) | (1 << k); + result[index++] = makeSME(syndrome(v), v); + } + } + } + + result = Golay24_detail::sort(result); + + std::array tmp; + for (size_t i = 0; i != LUT_SIZE; ++i) + { + tmp[i] = makeSyndromeMapEntry(result[i]); + } + + return tmp; +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Golay24.h b/plugins/channelrx/demodm17/m17/Golay24.h new file mode 100644 index 000000000..10a28fad5 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Golay24.h @@ -0,0 +1,109 @@ +// Copyright 2020 Rob Riggs +// All rights reserved. + +#pragma once + +#include +#include +#include +#include +#include + +namespace mobilinkd { + +// Parts are adapted from: +// http://aqdi.com/articles/using-the-golay-error-detection-and-correction-code-3/ + + +namespace Golay24_detail +{ + +// Need a constexpr sort. +// https://stackoverflow.com/a/40030044/854133 +template +constexpr void swap(T& l, T& r) +{ + T tmp = std::move(l); + l = std::move(r); + r = std::move(tmp); +} + +template +struct array +{ + constexpr T& operator[](size_t i) { + return arr[i]; + } + + constexpr const T& operator[](size_t i) const { + return arr[i]; + } + + constexpr const T* begin() const { + return arr; + } + + constexpr const T* end() const { + return arr + N; + } + + T arr[N]; +}; + +template +constexpr void sort_impl(array &array, size_t left, size_t right) +{ + if (left < right) + { + size_t m = left; + + for (size_t i = left + 1; i +constexpr array sort(array array) +{ + auto sorted = array; + sort_impl(sorted, 0, N); + return sorted; +} + +} // Golay24_detail + + +struct Golay24 +{ + #pragma pack(push, 1) + struct SyndromeMapEntry + { + uint32_t a{0}; + uint16_t b{0}; + }; + #pragma pack(pop) + + static const uint16_t POLY = 0xC75; + static const size_t LUT_SIZE = 2048; + static std::array LUT; + + Golay24(); + static uint32_t encode23(uint16_t data); + static uint32_t encode24(uint16_t data); + static bool decode(uint32_t input, uint32_t& output); + +private: + static bool parity(uint32_t codeword); + static SyndromeMapEntry makeSyndromeMapEntry(uint64_t val); + static uint32_t syndrome(uint32_t codeword); + static uint64_t makeSME(uint64_t syndrome, uint32_t bits); + static std::array make_lut(); +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/IirFilter.h b/plugins/channelrx/demodm17/m17/IirFilter.h new file mode 100644 index 000000000..947c38d83 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/IirFilter.h @@ -0,0 +1,52 @@ +// Copyright 2015-2021 Mobilinkd LLC. + +#pragma once + +#include "Filter.h" + +#include +#include + +namespace mobilinkd +{ + +template +struct BaseIirFilter : FilterBase +{ + const std::array& numerator_; + const std::array denominator_; + std::array history_{0}; + + BaseIirFilter(const std::array& b, const std::array& a) + : numerator_(b), denominator_(a) + { + history_.fill(0.0); + } + + FloatType operator()(FloatType input) { + + for (size_t i = N - 1; i != 0; i--) history_[i] = history_[i - 1]; + + history_[0] = input; + + for (size_t i = 1; i != N; i++) { + history_[0] -= denominator_[i] * history_[i]; + } + + FloatType result = 0; + for (size_t i = 0; i != N; i++) { + result += numerator_[i] * history_[i]; + } + + return result; + } +}; + +template +BaseIirFilter makeIirFilter( + const std::array& b, const std::array& a) +{ + return std::move(BaseIirFilter(b, a)); +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/LinkSetupFrame.h b/plugins/channelrx/demodm17/m17/LinkSetupFrame.h new file mode 100644 index 000000000..96c39c099 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/LinkSetupFrame.h @@ -0,0 +1,131 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include // Don't have std::span in C++17. +#include +#include + +namespace mobilinkd +{ + +struct LinkSetupFrame +{ + using call_t = std::array; // NUL-terminated C-string. + using encoded_call_t = std::array; + using frame_t = std::array; + + static constexpr encoded_call_t BROADCAST_ADDRESS = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + static constexpr call_t BROADCAST_CALL = {'B', 'R', 'O', 'A', 'D', 'C', 'A', 'S', 'T', 0}; + + enum TxType { PACKET, STREAM }; + enum DataType { DT_RESERVED, DATA, VOICE, MIXED }; + enum EncType { NONE, AES, LFSR, ET_RESERVED }; + + call_t tocall_ = {0}; // Destination + call_t mycall_ = {0}; // Source + TxType tx_type_ = TxType::STREAM; + DataType data_type_ = DataType::VOICE; + EncType encryption_type_ = EncType::NONE; + + /** + * The callsign is encoded in base-40 starting with the right-most + * character. The final value is written out in "big-endian" form, with + * the most-significant value first. This leads to 0-padding of shorter + * callsigns. + * + * @param[in] callsign is the callsign to encode. + * @param[in] strict is a flag (disabled by default) which indicates whether + * invalid characters are allowed and assugned a value of 0 or not allowed, + * resulting in an exception. + * @return the encoded callsign as an array of 6 bytes. + * @throw invalid_argument when strict is true and an invalid callsign (one + * containing an unmappable character) is passed. + */ + static encoded_call_t encode_callsign(call_t callsign, bool strict = false) + { + // Encode the characters to base-40 digits. + uint64_t encoded = 0; + + std::reverse(callsign.begin(), callsign.end()); + + for (auto c : callsign) + { + encoded *= 40; + if (c >= 'A' and c <= 'Z') + { + encoded += c - 'A' + 1; + } + else if (c >= '0' and c <= '9') + { + encoded += c - '0' + 27; + } + else if (c == '-') + { + encoded += 37; + } + else if (c == '/') + { + encoded += 38; + } + else if (c == '.') + { + encoded += 39; + } + else if (strict) + { + throw std::invalid_argument("bad callsign"); + } + } + const auto p = reinterpret_cast(&encoded); + + encoded_call_t result; + std::copy(p, p + 6, result.rbegin()); + + return result; + } + + /** + * Decode a base-40 encoded callsign to its text representation. This decodes + * a 6-byte big-endian value into a string of up to 9 characters. + */ + static call_t decode_callsign(encoded_call_t callsign) + { + static const char callsign_map[] = "xABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789-/."; + + call_t result; + + if (callsign == BROADCAST_ADDRESS) + { + result = BROADCAST_CALL; + return result; + } + + uint64_t encoded = 0; // This only works on little endian architectures. + auto p = reinterpret_cast(&encoded); + std::copy(callsign.rbegin(), callsign.rend(), p); + + // decode each base-40 digit and map them to the appriate character. + result.fill(0); + size_t index = 0; + while (encoded) + { + result[index++] = callsign_map[encoded % 40]; + encoded /= 40; + } + + return result; + } + + LinkSetupFrame() + {} + + LinkSetupFrame& myCall(const char*) + { + return *this; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Demodulator.cpp b/plugins/channelrx/demodm17/m17/M17Demodulator.cpp new file mode 100644 index 000000000..1754df4f7 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Demodulator.cpp @@ -0,0 +1,89 @@ +#include "M17Demodulator.h" + +namespace mobilinkd { + +template <> +const std::array M17Demodulator::rrc_taps = std::array{ + 0.0029364388513841593, 0.0031468394550958484, 0.002699564567597445, 0.001661182944400927, + 0.00023319405581230247, -0.0012851320781224025, -0.0025577136087664687, -0.0032843366522956313, + -0.0032697038088887226, -0.0024733964729590865, -0.0010285696910973807, 0.0007766690889758685, + 0.002553421969211845, 0.0038920145144327816, 0.004451886520053017, 0.00404219185231544, + 0.002674727068399207, 0.0005756567993179152, -0.0018493784971116507, -0.004092346891623224, + -0.005648131453822014, -0.006126925416243605, -0.005349511529163396, -0.003403189203405097, + -0.0006430502751187517, 0.002365929161655135, 0.004957956568090113, 0.006506845894531803, + 0.006569574194782443, 0.0050017573119839134, 0.002017321931508163, -0.0018256054303579805, + -0.00571615173291049, -0.008746639552588416, -0.010105075751866371, -0.009265784007800534, + -0.006136551625729697, -0.001125978562075172, 0.004891777252042491, 0.01071805138282269, + 0.01505751553351295, 0.01679337935001369, 0.015256245142156299, 0.01042830577908502, + 0.003031522725559901, -0.0055333532968188165, -0.013403099825723372, -0.018598682349642525, + -0.01944761739590459, -0.015005271935951746, -0.0053887880354343935, 0.008056525910253532, + 0.022816244158307273, 0.035513467692208076, 0.04244131815783876, 0.04025481153629372, + 0.02671818654865632, 0.0013810216516704976, -0.03394615682795165, -0.07502635967975885, + -0.11540977897637611, -0.14703962203941534, -0.16119995609538576, -0.14969512896336504, + -0.10610329539459686, -0.026921412469634916, 0.08757875030779196, 0.23293327870303457, + 0.4006012210123992, 0.5786324696325503, 0.7528286479934068, 0.908262741447522, + 1.0309661131633199, 1.1095611856548013, 1.1366197723675815, 1.1095611856548013, + 1.0309661131633199, 0.908262741447522, 0.7528286479934068, 0.5786324696325503, + 0.4006012210123992, 0.23293327870303457, 0.08757875030779196, -0.026921412469634916, + -0.10610329539459686, -0.14969512896336504, -0.16119995609538576, -0.14703962203941534, + -0.11540977897637611, -0.07502635967975885, -0.03394615682795165, 0.0013810216516704976, + 0.02671818654865632, 0.04025481153629372, 0.04244131815783876, 0.035513467692208076, + 0.022816244158307273, 0.008056525910253532, -0.0053887880354343935, -0.015005271935951746, + -0.01944761739590459, -0.018598682349642525, -0.013403099825723372, -0.0055333532968188165, + 0.003031522725559901, 0.01042830577908502, 0.015256245142156299, 0.01679337935001369, + 0.01505751553351295, 0.01071805138282269, 0.004891777252042491, -0.001125978562075172, + -0.006136551625729697, -0.009265784007800534, -0.010105075751866371, -0.008746639552588416, + -0.00571615173291049, -0.0018256054303579805, 0.002017321931508163, 0.0050017573119839134, + 0.006569574194782443, 0.006506845894531803, 0.004957956568090113, 0.002365929161655135, + -0.0006430502751187517, -0.003403189203405097, -0.005349511529163396, -0.006126925416243605, + -0.005648131453822014, -0.004092346891623224, -0.0018493784971116507, 0.0005756567993179152, + 0.002674727068399207, 0.00404219185231544, 0.004451886520053017, 0.0038920145144327816, + 0.002553421969211845, 0.0007766690889758685, -0.0010285696910973807, -0.0024733964729590865, + -0.0032697038088887226, -0.0032843366522956313, -0.0025577136087664687, -0.0012851320781224025, + 0.00023319405581230247, 0.001661182944400927, 0.002699564567597445, 0.0031468394550958484, + 0.0029364388513841593, 0.0 +}; + +template <> +const std::array M17Demodulator::rrc_taps = std::array{ + 0.0029364388513841593, 0.0031468394550958484, 0.002699564567597445, 0.001661182944400927, + 0.00023319405581230247, -0.0012851320781224025, -0.0025577136087664687, -0.0032843366522956313, + -0.0032697038088887226, -0.0024733964729590865, -0.0010285696910973807, 0.0007766690889758685, + 0.002553421969211845, 0.0038920145144327816, 0.004451886520053017, 0.00404219185231544, + 0.002674727068399207, 0.0005756567993179152, -0.0018493784971116507, -0.004092346891623224, + -0.005648131453822014, -0.006126925416243605, -0.005349511529163396, -0.003403189203405097, + -0.0006430502751187517, 0.002365929161655135, 0.004957956568090113, 0.006506845894531803, + 0.006569574194782443, 0.0050017573119839134, 0.002017321931508163, -0.0018256054303579805, + -0.00571615173291049, -0.008746639552588416, -0.010105075751866371, -0.009265784007800534, + -0.006136551625729697, -0.001125978562075172, 0.004891777252042491, 0.01071805138282269, + 0.01505751553351295, 0.01679337935001369, 0.015256245142156299, 0.01042830577908502, + 0.003031522725559901, -0.0055333532968188165, -0.013403099825723372, -0.018598682349642525, + -0.01944761739590459, -0.015005271935951746, -0.0053887880354343935, 0.008056525910253532, + 0.022816244158307273, 0.035513467692208076, 0.04244131815783876, 0.04025481153629372, + 0.02671818654865632, 0.0013810216516704976, -0.03394615682795165, -0.07502635967975885, + -0.11540977897637611, -0.14703962203941534, -0.16119995609538576, -0.14969512896336504, + -0.10610329539459686, -0.026921412469634916, 0.08757875030779196, 0.23293327870303457, + 0.4006012210123992, 0.5786324696325503, 0.7528286479934068, 0.908262741447522, + 1.0309661131633199, 1.1095611856548013, 1.1366197723675815, 1.1095611856548013, + 1.0309661131633199, 0.908262741447522, 0.7528286479934068, 0.5786324696325503, + 0.4006012210123992, 0.23293327870303457, 0.08757875030779196, -0.026921412469634916, + -0.10610329539459686, -0.14969512896336504, -0.16119995609538576, -0.14703962203941534, + -0.11540977897637611, -0.07502635967975885, -0.03394615682795165, 0.0013810216516704976, + 0.02671818654865632, 0.04025481153629372, 0.04244131815783876, 0.035513467692208076, + 0.022816244158307273, 0.008056525910253532, -0.0053887880354343935, -0.015005271935951746, + -0.01944761739590459, -0.018598682349642525, -0.013403099825723372, -0.0055333532968188165, + 0.003031522725559901, 0.01042830577908502, 0.015256245142156299, 0.01679337935001369, + 0.01505751553351295, 0.01071805138282269, 0.004891777252042491, -0.001125978562075172, + -0.006136551625729697, -0.009265784007800534, -0.010105075751866371, -0.008746639552588416, + -0.00571615173291049, -0.0018256054303579805, 0.002017321931508163, 0.0050017573119839134, + 0.006569574194782443, 0.006506845894531803, 0.004957956568090113, 0.002365929161655135, + -0.0006430502751187517, -0.003403189203405097, -0.005349511529163396, -0.006126925416243605, + -0.005648131453822014, -0.004092346891623224, -0.0018493784971116507, 0.0005756567993179152, + 0.002674727068399207, 0.00404219185231544, 0.004451886520053017, 0.0038920145144327816, + 0.002553421969211845, 0.0007766690889758685, -0.0010285696910973807, -0.0024733964729590865, + -0.0032697038088887226, -0.0032843366522956313, -0.0025577136087664687, -0.0012851320781224025, + 0.00023319405581230247, 0.001661182944400927, 0.002699564567597445, 0.0031468394550958484, + 0.0029364388513841593, 0.0 +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Demodulator.h b/plugins/channelrx/demodm17/m17/M17Demodulator.h new file mode 100644 index 000000000..cae74f8dc --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Demodulator.h @@ -0,0 +1,589 @@ +// Copyright 2020-2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include "ClockRecovery.h" +#include "Correlator.h" +#include "DataCarrierDetect.h" +#include "FirFilter.h" +#include "FreqDevEstimator.h" +#include "M17FrameDecoder.h" +#include "M17Framer.h" +#include "Util.h" + +#include +#include +#include +#include +#include + +namespace mobilinkd { + +namespace detail +{ + + +} // detail + +template +struct M17Demodulator +{ + static const uint16_t SAMPLE_RATE = 48000; + static const uint16_t SYMBOL_RATE = 4800; + static const uint16_t SAMPLES_PER_SYMBOL = SAMPLE_RATE / SYMBOL_RATE; + static const uint16_t BLOCK_SIZE = 192; + + static constexpr FloatType sample_rate = SAMPLE_RATE; + static constexpr FloatType symbol_rate = SYMBOL_RATE; + + static const uint8_t MAX_MISSING_SYNC = 8; + + using collelator_t = Correlator; + using sync_word_t = SyncWord; + using callback_t = M17FrameDecoder::callback_t; + using diagnostic_callback_t = std::function; + + enum class DemodState { + UNLOCKED, + LSF_SYNC, + STREAM_SYNC, + PACKET_SYNC, + BERT_SYNC, + FRAME + }; + + DataCarrierDetect dcd{2500, 4000, 1.0, 4.0}; + ClockRecovery clock_recovery; + + collelator_t correlator; + sync_word_t preamble_sync{{+3,-3,+3,-3,+3,-3,+3,-3}, 29.f}; + sync_word_t lsf_sync{{+3,+3,+3,+3,-3,-3,+3,-3}, 32.f, -31.f}; // LSF or STREAM (inverted) + sync_word_t packet_sync{{3,-3,3,3,-3,-3,-3,-3}, 31.f, -31.f}; // PACKET or BERT (inverted) + + FreqDevEstimator dev; + FloatType idev; + size_t count_ = 0; + + int8_t polarity = 1; + M17Framer<368> framer; + M17FrameDecoder decoder; + DemodState demodState = DemodState::UNLOCKED; + M17FrameDecoder::SyncWordType sync_word_type = M17FrameDecoder::SyncWordType::LSF; + uint8_t sample_index = 0; + + bool dcd_ = false; + bool need_clock_reset_ = false; + bool need_clock_update_ = false; + + bool passall_ = false; + int viterbi_cost = 0; + int sync_count = 0; + int missing_sync_count = 0; + uint8_t sync_sample_index = 0; + diagnostic_callback_t diagnostic_callback; + + M17Demodulator(callback_t callback) : + decoder(callback) + {} + + virtual ~M17Demodulator() {} + + void dcd_on(); + void dcd_off(); + void initialize(const FloatType input); + void update_dcd(); + void do_unlocked(); + void do_lsf_sync(); + void do_packet_sync(); + void do_stream_sync(); + void do_bert_sync(); + void do_frame(FloatType filtered_sample); + + bool locked() const + { + return dcd_; + } + + void passall(bool enabled) + { + passall_ = enabled; + // decoder.passall(enabled); + } + + void diagnostics(diagnostic_callback_t callback) + { + diagnostic_callback = callback; + } + + void update_values(uint8_t index); + + void operator()(const FloatType input); + +private: + static const std::array rrc_taps; + BaseFirFilter demod_filter{rrc_taps}; +}; + +template +void M17Demodulator::update_values(uint8_t index) +{ + correlator.apply([this,index](FloatType t){dev.sample(t);}, index); + dev.update(); + sync_sample_index = index; +} + +template +void M17Demodulator::dcd_on() +{ + // Data carrier newly detected. + dcd_ = true; + sync_count = 0; + missing_sync_count = 0; + + dev.reset(); + framer.reset(); + decoder.reset(); +} + +template +void M17Demodulator::dcd_off() +{ + // Just lost data carrier. + dcd_ = false; + demodState = DemodState::UNLOCKED; + decoder.reset(); + + if (diagnostic_callback) + { + diagnostic_callback(int(dcd_), dev.error(), dev.deviation(), dev.offset(), (int) demodState, + clock_recovery.clock_estimate(), sample_index, sync_sample_index, clock_recovery.sample_index(), -1); + } +} + +template +void M17Demodulator::initialize(const FloatType input) +{ + auto filtered_sample = demod_filter(input); + correlator.sample(filtered_sample); +} + +template +void M17Demodulator::update_dcd() +{ + if (!dcd_ && dcd.dcd()) + { + // fputs("\nAOS\n", stderr); + dcd_on(); + need_clock_reset_ = true; + } + else if (dcd_ && !dcd.dcd()) + { + // fputs("\nLOS\n", stderr); + dcd_off(); + } +} + +template +void M17Demodulator::do_unlocked() +{ + // We expect to find the preamble immediately after DCD. + if (missing_sync_count < 1920) + { + missing_sync_count += 1; + auto sync_index = preamble_sync(correlator); + auto sync_updated = preamble_sync.updated(); + if (sync_updated) + { + sync_count = 0; + missing_sync_count = 0; + need_clock_reset_ = true; + dev.reset(); + update_values(sync_index); + sample_index = sync_index; + demodState = DemodState::LSF_SYNC; + } + return; + } + auto sync_index = lsf_sync(correlator); + auto sync_updated = lsf_sync.updated(); + if (sync_updated) + { + sync_count = 0; + missing_sync_count = 0; + need_clock_reset_ = true; + dev.reset(); + update_values(sync_index); + sample_index = sync_index; + demodState = DemodState::FRAME; + if (sync_updated < 0) + { + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + } + else + { + sync_word_type = M17FrameDecoder::SyncWordType::LSF; + } + return; + } + sync_index = packet_sync(correlator); + sync_updated = packet_sync.updated(); + if (sync_updated < 0) + { + sync_count = 0; + missing_sync_count = 0; + need_clock_reset_ = true; + dev.reset(); + update_values(sync_index); + sample_index = sync_index; + demodState = DemodState::FRAME; + sync_word_type = M17FrameDecoder::SyncWordType::BERT; + } +} + +/** + * Check for LSF sync word. We only enter the DemodState::LSF_SYNC state + * if a preamble sync has been detected, which also means that sample_index + * has been initialized to a sane value for the baseband. + */ +template +void M17Demodulator::do_lsf_sync() +{ + FloatType sync_triggered = 0.; + FloatType bert_triggered = 0.; + + if (correlator.index() == sample_index) + { + sync_triggered = preamble_sync.triggered(correlator); + if (sync_triggered > 0.1) + { + return; + } + sync_triggered = lsf_sync.triggered(correlator); + bert_triggered = packet_sync.triggered(correlator); + if (bert_triggered < 0) + { + missing_sync_count = 0; + need_clock_update_ = true; + update_values(sample_index); + demodState = DemodState::FRAME; + sync_word_type = M17FrameDecoder::SyncWordType::BERT; + } + else if (std::abs(sync_triggered) > 0.1) + { + missing_sync_count = 0; + need_clock_update_ = true; + update_values(sample_index); + if (sync_triggered > 0) + { + demodState = DemodState::FRAME; + sync_word_type = M17FrameDecoder::SyncWordType::LSF; + } + else + { + demodState = DemodState::FRAME; + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + } + } + else if (++missing_sync_count > 192) + { + demodState = DemodState::UNLOCKED; + decoder.reset(); + missing_sync_count = 0; + } + else + { + update_values(sample_index); + } + } +} + +/** + * Check for a stream sync word (LSF sync word that is maximally negative). + * We can enter DemodState::STREAM_SYNC from either a valid LSF decode for + * an audio stream, or from a stream frame decode. + * + */ +template +void M17Demodulator::do_stream_sync() +{ + uint8_t sync_index = lsf_sync(correlator); + int8_t sync_updated = lsf_sync.updated(); + sync_count += 1; + if (sync_updated < 0) // Stream sync word + { + missing_sync_count = 0; + if (sync_count > 70) + { + update_values(sync_index); + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + demodState = DemodState::FRAME; + } + return; + } + else if (sync_count > 87) + { + update_values(sync_index); + missing_sync_count += 1; + if (missing_sync_count < MAX_MISSING_SYNC) + { + sync_word_type = M17FrameDecoder::SyncWordType::STREAM; + demodState = DemodState::FRAME; + } + else + { + // fputs("\n!SYNC\n", stderr); + demodState = DemodState::LSF_SYNC; + } + } +} + +/** + * Check for a packet sync word. DemodState::PACKET_SYNC can only be + * entered from a valid LSF frame decode with the data/packet type bit set. + */ +template +void M17Demodulator::do_packet_sync() +{ + auto sync_index = packet_sync(correlator); + auto sync_updated = packet_sync.updated(); + sync_count += 1; + if (sync_count > 70 && sync_updated) + { + missing_sync_count = 0; + update_values(sync_index); + sync_word_type = M17FrameDecoder::SyncWordType::PACKET; + demodState = DemodState::FRAME; + } + else if (sync_count > 87) + { + missing_sync_count += 1; + if (missing_sync_count < MAX_MISSING_SYNC) + { + sync_word_type = M17FrameDecoder::SyncWordType::PACKET; + demodState = DemodState::FRAME; + } + else + { + demodState = DemodState::UNLOCKED; + decoder.reset(); + } + } +} + +/** + * Check for a bert sync word. + */ +template +void M17Demodulator::do_bert_sync() +{ + auto sync_index = packet_sync(correlator); + auto sync_updated = packet_sync.updated(); + sync_count += 1; + if (sync_count > 70 && sync_updated < 0) + { + missing_sync_count = 0; + update_values(sync_index); + sync_word_type = M17FrameDecoder::SyncWordType::BERT; + demodState = DemodState::FRAME; + } + else if (sync_count > 87) + { + missing_sync_count += 1; + if (missing_sync_count < MAX_MISSING_SYNC) + { + sync_word_type = M17FrameDecoder::SyncWordType::BERT; + demodState = DemodState::FRAME; + } + else + { + demodState = DemodState::UNLOCKED; + decoder.reset(); + } + } +} + + +template +void M17Demodulator::do_frame(FloatType filtered_sample) +{ + if (correlator.index() != sample_index) return; + + static uint8_t cost_count = 0; + + auto sample = filtered_sample - dev.offset(); + sample *= dev.idev(); + sample *= polarity; + + auto n = llr(sample); + int8_t* tmp; + auto len = framer(n, &tmp); + if (len != 0) + { + need_clock_update_ = true; + + M17FrameDecoder::input_buffer_t buffer; + std::copy(tmp, tmp + len, buffer.begin()); + auto valid = decoder(sync_word_type, buffer, viterbi_cost); + + cost_count = viterbi_cost > 90 ? cost_count + 1 : 0; + cost_count = viterbi_cost > 100 ? cost_count + 1 : cost_count; + cost_count = viterbi_cost > 110 ? cost_count + 1 : cost_count; + + if (cost_count > 75) + { + cost_count = 0; + demodState = DemodState::UNLOCKED; + decoder.reset(); + // fputs("\nCOST\n", stderr); + return; + } + + switch (decoder.state()) + { + case M17FrameDecoder::State::STREAM: + demodState = DemodState::STREAM_SYNC; + break; + case M17FrameDecoder::State::LSF: + // If state == LSF, we need to recover LSF from LICH. + demodState = DemodState::STREAM_SYNC; + break; + case M17FrameDecoder::State::BERT: + demodState = DemodState::BERT_SYNC; + break; + default: + demodState = DemodState::PACKET_SYNC; + break; + } + + sync_count = 0; + + switch (valid) + { + case M17FrameDecoder::DecodeResult::FAIL: + break; + case M17FrameDecoder::DecodeResult::EOS: + demodState = DemodState::LSF_SYNC; + break; + case M17FrameDecoder::DecodeResult::OK: + break; + case M17FrameDecoder::DecodeResult::INCOMPLETE: + break; + case M17FrameDecoder::DecodeResult::PACKET_INCOMPLETE: + break; + } + } +} + +template +void M17Demodulator::operator()(const FloatType input) +{ + static int16_t initializing = 1920; + + count_++; + + dcd(input); + + // We need to pump a few ms of data through on startup to initialize + // the demodulator. + if (initializing) [[unlikely]] + { + --initializing; + initialize(input); + count_ = 0; + return; + } + + if (!dcd_) + { + if (count_ % (BLOCK_SIZE * 2) == 0) + { + update_dcd(); + dcd.update(); + + if (diagnostic_callback) + { + diagnostic_callback(int(dcd_), dev.error(), dev.deviation(), dev.offset(), (int) demodState, + clock_recovery.clock_estimate(), sample_index, sync_sample_index, clock_recovery.sample_index(), viterbi_cost); + } + + count_ = 0; + } + + return; + } + + auto filtered_sample = demod_filter(input); + + correlator.sample(filtered_sample); + + if (correlator.index() == 0) + { + if (need_clock_reset_) + { + clock_recovery.reset(); + need_clock_reset_ = false; + } + else if (need_clock_update_) // must avoid update immediately after reset. + { + clock_recovery.update(); + uint8_t clock_index = clock_recovery.sample_index(); + uint8_t clock_diff = std::abs(sample_index - clock_index); + uint8_t sync_diff = std::abs(sample_index - sync_sample_index); + bool clock_diff_ok = clock_diff <= 1 || clock_diff == 9; + bool sync_diff_ok = sync_diff <= 1 || sync_diff == 9; + if (clock_diff_ok) sample_index = clock_index; + else if (sync_diff_ok) sample_index = sync_sample_index; + // else unchanged. + need_clock_update_ = false; + } + } + + clock_recovery(filtered_sample); + + if (demodState != DemodState::UNLOCKED && correlator.index() == sample_index) + { + dev.sample(filtered_sample); + } + + switch (demodState) + { + case DemodState::UNLOCKED: + // In this state, the sample_index is unknown. We need to find + // a sync word to find the proper sample_index. We only leave + // this state if we believe that we have a valid sample_index. + do_unlocked(); + break; + case DemodState::LSF_SYNC: + do_lsf_sync(); + break; + case DemodState::STREAM_SYNC: + do_stream_sync(); + break; + case DemodState::PACKET_SYNC: + do_packet_sync(); + break; + case DemodState::BERT_SYNC: + do_bert_sync(); + break; + case DemodState::FRAME: + do_frame(filtered_sample); + break; + } + + if (count_ % (BLOCK_SIZE * 5) == 0) + { + update_dcd(); + count_ = 0; + + if (diagnostic_callback) + { + diagnostic_callback(int(dcd_), dev.error(), dev.deviation(), dev.offset(), (int) demodState, + clock_recovery.clock_estimate(), sample_index, sync_sample_index, clock_recovery.sample_index(), viterbi_cost); + } + + dcd.update(); + } +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17FrameDecoder.h b/plugins/channelrx/demodm17/m17/M17FrameDecoder.h new file mode 100644 index 000000000..f639dd921 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17FrameDecoder.h @@ -0,0 +1,397 @@ +// Copyright 2021 Mobilinkd LLC. + +#pragma once + +#include "M17Randomizer.h" +#include "PolynomialInterleaver.h" +#include "Trellis.h" +#include "Viterbi.h" +#include "CRC16.h" +#include "LinkSetupFrame.h" +#include "Golay24.h" + +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + + +template +void dump(const std::array& data, char header = 'D') +{ + putchar(header); + putchar('='); + for (auto c : data) + { + const char hex[] = "0123456789ABCDEF"; + putchar(hex[uint8_t(c)>>4]); + putchar(hex[uint8_t(c)&0xf]); + } + putchar('\r'); + putchar('\n'); +} + +struct M17FrameDecoder +{ + static constexpr size_t MAX_LICH_FRAGMENT = 5; + + M17Randomizer<368> derandomize_; + PolynomialInterleaver<45, 92, 368> interleaver_; + Trellis<4,2> trellis_{makeTrellis<4, 2>({031,027})}; + Viterbi viterbi_{trellis_}; + CRC16<0x5935, 0xFFFF> crc_; + + enum class State { LSF, STREAM, BASIC_PACKET, FULL_PACKET, BERT }; + enum class SyncWordType { LSF, STREAM, PACKET, BERT }; + enum class DecodeResult { FAIL, OK, EOS, INCOMPLETE, PACKET_INCOMPLETE }; + enum class FrameType { LSF, LICH, STREAM, BASIC_PACKET, FULL_PACKET, BERT }; + + State state_ = State::LSF; + + using input_buffer_t = std::array; + + using lsf_conv_buffer_t = std::array; + using audio_conv_buffer_t = std::array; + + using lsf_buffer_t = std::array; + using lich_buffer_t = std::array; + using audio_buffer_t = std::array; + using packet_buffer_t = std::array; + using bert_buffer_t = std::array; + + using output_buffer_t = struct { + FrameType type; + union { + lich_buffer_t lich; + audio_buffer_t stream; + packet_buffer_t packet; + bert_buffer_t bert; + }; + lsf_buffer_t lsf; + }; + + using depunctured_buffer_t = union { + std::array lsf; + std::array stream; + std::array packet; + std::array bert; + }; + + using decode_buffer_t = union { + std::array lsf; + std::array stream; + std::array packet; + std::array bert; + }; + + /** + * Callback function for frame types. The caller is expected to return + * true if the data was good or unknown and false if the data is known + * to be bad. + */ + using callback_t = std::function; + + callback_t callback_; + + output_buffer_t output_buffer; + depunctured_buffer_t depuncture_buffer; + decode_buffer_t decode_buffer; + uint16_t frame_number = 0; + + uint8_t lich_segments{0}; ///< one bit per received LICH fragment. + + M17FrameDecoder(callback_t callback) + : callback_(callback) + {} + + void update_state(std::array& lsf_output) + { + if (lsf_output[111]) // LSF type bit 0 + { + if (lsf_output[109] != 0) { + state_ = State::STREAM; + } + } + else // packet frame comes next. + { + uint8_t packet_type = (lsf_output[109] << 1) | lsf_output[110]; + switch (packet_type) + { + case 1: // RAW -- ignore LSF. + state_ = State::BASIC_PACKET; + break; + case 2: // ENCAPSULATED + state_ = State::FULL_PACKET; + break; + default: + state_ = State::FULL_PACKET; + } + } + } + + void reset() + { + state_ = State::LSF; + frame_number = 0; + } + + /** + * Decode the LSF and, if it is valid, transition to the next state. + * + * The LSF is returned for STREAM mode, dropped for BASIC_PACKET mode, + * and captured for FULL_PACKET mode. + * + * @param buffer + * @param viterbi_cost + * @return + */ + DecodeResult decode_lsf(input_buffer_t&, int& viterbi_cost) + { + viterbi_cost = viterbi_.decode(depuncture_buffer.lsf, decode_buffer.lsf); + to_byte_array(decode_buffer.lsf, output_buffer.lsf); + + // dump(output_buffer.lsf); + // printf("cost = %lu\n", viterbi_cost); + + crc_.reset(); + for (auto c : output_buffer.lsf) crc_(c); + auto checksum = crc_.get(); + + if (checksum == 0) + { + update_state(decode_buffer.lsf); + output_buffer.type = FrameType::LSF; + callback_(output_buffer, viterbi_cost); + return DecodeResult::OK; + } + + lich_segments = 0; + output_buffer.lsf.fill(0); + return DecodeResult::FAIL; + } + + // Unpack & decode LICH fragments into tmp_buffer. + bool unpack_lich(input_buffer_t& buffer) + { + size_t index = 0; + // Read the 4 24-bit codewords from LICH + for (size_t i = 0; i != 4; ++i) // for each codeword + { + uint32_t codeword = 0; + for (size_t j = 0; j != 24; ++j) // for each bit in codeword + { + codeword <<= 1; + codeword |= (buffer[i * 24 + j] > 0); + } + uint32_t decoded = 0; + if (!Golay24::decode(codeword, decoded)) + { + return false; + } + decoded >>= 12; // Remove check bits and parity. + // append codeword. + if (i & 1) + { + output_buffer.lich[index++] |= (decoded >> 8); // upper 4 bits + output_buffer.lich[index++] = (decoded & 0xFF); // lower 8 bits + } + else + { + output_buffer.lich[index++] |= (decoded >> 4); // upper 8 bits + output_buffer.lich[index] = (decoded & 0x0F) << 4; // lower 4 bits + } + } + return true; + } + + DecodeResult decode_lich(input_buffer_t& buffer, int& viterbi_cost) + { + output_buffer.lich.fill(0); + // Read the 4 12-bit codewords from LICH into buffers.lich. + if (!unpack_lich(buffer)) return DecodeResult::FAIL; + + output_buffer.type = FrameType::LICH; + callback_(output_buffer, 0); + + uint8_t fragment_number = output_buffer.lich[5]; // Get fragment number. + fragment_number = (fragment_number >> 5) & 7; + + if (fragment_number > MAX_LICH_FRAGMENT) + { + viterbi_cost = -1; + return DecodeResult::INCOMPLETE; // More to go... + } + + // Copy decoded LICH to superframe buffer. + std::copy(output_buffer.lich.begin(), output_buffer.lich.begin() + 5, + output_buffer.lsf.begin() + (fragment_number * 5)); + + lich_segments |= (1 << fragment_number); // Indicate segment received. + if ((lich_segments & 0x3F) != 0x3F) + { + viterbi_cost = -1; + return DecodeResult::INCOMPLETE; // More to go... + } + + crc_.reset(); + for (auto c : output_buffer.lsf) crc_(c); + auto checksum = crc_.get(); + + if (checksum == 0) + { + lich_segments = 0; + state_ = State::STREAM; + viterbi_cost = 0; + output_buffer.type = FrameType::LSF; + callback_(output_buffer, viterbi_cost); + return DecodeResult::OK; + } + + // Failed CRC... try again. + // lich_segments = 0; + // output_buffer.lsf.fill(0); + viterbi_cost = 128; + return DecodeResult::INCOMPLETE; + } + + DecodeResult decode_bert(input_buffer_t&, int& viterbi_cost) + { + viterbi_cost = viterbi_.decode(depuncture_buffer.bert, decode_buffer.bert); + to_byte_array(decode_buffer.bert, output_buffer.bert); + + output_buffer.type = FrameType::BERT; + callback_(output_buffer, viterbi_cost); + + return DecodeResult::OK; + } + + DecodeResult decode_stream(input_buffer_t& buffer, int& viterbi_cost) + { + std::array tmp; + std::copy(buffer.begin() + 96, buffer.end(), tmp.begin()); + + viterbi_cost = viterbi_.decode(depuncture_buffer.stream, decode_buffer.stream); + to_byte_array(decode_buffer.stream, output_buffer.stream); + + if ((viterbi_cost < 60) && (output_buffer.stream[0] & 0x80)) + { + // fputs("\nEOS\n", stderr); + state_ = State::LSF; + } + + output_buffer.type = FrameType::STREAM; + callback_(output_buffer, viterbi_cost); + + return state_ == State::LSF ? DecodeResult::EOS : DecodeResult::OK; + } + + /** + * Capture packet frames until an EOF bit is found. + + * @param buffer the demodulated M17 symbols in LLR format. + * @param viterbi_cost the cost of traversing the trellis. + * @param frame_type is either BASIC_PACKET or FULL_PACKET. + * @return the result of decoding the packet frame. + */ + DecodeResult decode_packet(input_buffer_t&, int& viterbi_cost, FrameType type) + { + viterbi_cost = viterbi_.decode(depuncture_buffer.packet, decode_buffer.packet); + to_byte_array(decode_buffer.packet, output_buffer.packet); + + output_buffer.type = type; + auto result = callback_(output_buffer, viterbi_cost); + + if (output_buffer.packet[25] & 0x80) // last packet; + { + state_ = State::LSF; + return result ? DecodeResult::OK : DecodeResult::FAIL; + } + + return DecodeResult::PACKET_INCOMPLETE; + } + + /** + * Decode M17 frames. The decoder uses the sync word to determine frame + * type and to update its state machine. + * + * The decoder receives M17 frame type indicator (based on sync word) and + * frames from the M17 demodulator. + * + * If the frame is an LSF, the state immediately changes to LSF. When + * in LSF mode, the state machine can transition to: + * + * - LSF if the CRC is bad. + * - STREAM if the LSF type field indicates Stream. + * - BASIC_PACKET if the LSF type field indicates Packet and the packet + * type is RAW. + * - FULL_PACKET if the LSF type field indicates Packet and the packet + * type is ENCAPSULATED or RESERVED. + * + * When in LSF mode, if an LSF frame is received it is parsed as an LSF. + * When a STREAM frame is received, it attempts to recover an LSF from + * the LICH. PACKET frame types are ignored when state is LSF. + * + * When in STREAM mode, the state machine can transition to either: + * + * - STREAM when a any stream frame is received. + * - LSF when the EOS indicator is set, or when a packet frame is received. + * + * When in BASIC_PACKET mode, the state machine can transition to either: + * + * - BASIC_PACKET when any packet frame is received. + * - LSF when the EOS indicator is set, or when a stream frame is received. + * + * When in FULL_PACKET mode, the state machine can transition to either: + * + * - FULL_PACKET when any packet frame is received. + * - LSF when the EOS indicator is set, or when a stream frame is received. + */ + DecodeResult operator()(SyncWordType frame_type, input_buffer_t& buffer, int& viterbi_cost) + { + derandomize_(buffer); + interleaver_.deinterleave(buffer); + + // This is out state machined. + switch(frame_type) + { + case SyncWordType::LSF: + state_ = State::LSF; + return decode_lsf(buffer, viterbi_cost); + case SyncWordType::STREAM: + switch (state_) + { + case State::LSF: + return decode_lich(buffer, viterbi_cost); + case State::STREAM: + return decode_stream(buffer, viterbi_cost); + default: + state_ = State::LSF; + } + break; + case SyncWordType::PACKET: + switch (state_) + { + case State::BASIC_PACKET: + return decode_packet(buffer, viterbi_cost, FrameType::BASIC_PACKET); + case State::FULL_PACKET: + return decode_packet(buffer, viterbi_cost, FrameType::FULL_PACKET); + default: + state_ = State::LSF; + } + break; + case SyncWordType::BERT: + state_ = State::BERT; + return decode_bert(buffer, viterbi_cost); + } + + return DecodeResult::FAIL; + } + + State state() const { return state_; } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Framer.h b/plugins/channelrx/demodm17/m17/M17Framer.h new file mode 100644 index 000000000..c80365e99 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Framer.h @@ -0,0 +1,62 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include +#include + +namespace mobilinkd +{ + +template +struct M17Framer +{ + using buffer_t = std::array; + + alignas(16) buffer_t buffer_; + size_t index_ = 0; + + M17Framer() + { + reset(); + } + + static constexpr size_t size() { return N; } + + size_t operator()(int dibit, int8_t** result) + { + buffer_[index_++] = (dibit >> 1) ? 1 : -1; + buffer_[index_++] = (dibit & 1) ? 1 : -1; + if (index_ == N) + { + index_ = 0; + *result = buffer_.data(); + return N; + } + return 0; + } + + // LLR mode + size_t operator()(std::tuple symbol, int8_t** result) + { + buffer_[index_++] = std::get<0>(symbol); + buffer_[index_++] = std::get<1>(symbol); + if (index_ == N) + { + index_ = 0; + *result = buffer_.data(); + return N; + } + return 0; + } + + void reset() + { + buffer_.fill(0); + index_ = 0; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Modulator.h b/plugins/channelrx/demodm17/m17/M17Modulator.h new file mode 100644 index 000000000..a8d8ee851 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Modulator.h @@ -0,0 +1,637 @@ +#pragma once + +#include "queue.h" +#include "FirFilter.h" +#include "LinkSetupFrame.h" +#include "CRC16.h" +#include "Convolution.h" +#include "PolynomialInterleaver.h" +#include "M17Randomizer.h" +#include "Util.h" +#include "Golay24.h" +#include "Trellis.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +/** + * Asynchronous M17 modulator. This modulator is initialized with the source and + * destination callsigns. It is then run by attaching an input queue and an output + * queue. The modulator reads 16-bit, 8ksps, 1-channel audio samples from the input + * queue and an M17 bitstream (in 8-bit bytes, 4 symbols per byte) to the output queue. + * + * The call to run(), which is used to attach the queues, returns immediately, starting + * a new thread in a detached state. run() returns a future, which may contain error + * information if an exception is thrown. + * + * The modulator stops when the input queue is closed. + * + * The modulator starts in a paused state, discarding all input. + * + * The modulator is started by calling ptt_on(). This causes the preamble and link + * setup frame to be sent. The modulator then starts reading from the input queue + * and writing the data stream to the output queue. + * + * The modulator can be paused by calling ptt_off(). This will cause any audio + * samples remaining in the input queue to be discarded. The final frame will + * be sent with the EOS bit set. The output queue should always be completely + * drained and all symbols output should be transmitted to ensure proper EOS + * signalling. + * + * Output will be bursty -- their is no throttling of the symbol stream. As soon + * as enough input samples are received to fill the M17 payload field, the frame + * will be constructed and the symbol stream output on the queue. + * + * @invariant The state of the modulator is one of INACTIVE, IDLE, PREAMBLE, + * LINK_SETUP, ACTIVE, or END_OF_STREAM. + * + * The modulator transitions from INACTIVE to IDLE when run() is called. + * + * The modulator transitions from IDLE to PREAMBLE when ptt_on() is called. + * + * The modulator will transition from PREAMBLE to LINK_SETUP to ACTIVE automatically. + * + * The modulator transitions from ACTIVE to END_OF_STREAM when ptt_off() is called. + * + * The modulator transitions from END_OF_STREAM to IDLE after the last audio + * frame is emitted. + * + * The modulator will transition from IDLE to INACTIVE when the input or output + * queue is closed. + * + * The modulator will emit at least 3 frames when ptt_on() is called: the preamble, + * the link setup frame, and one audio frame with the EOS flag set. + * + * It is an error to close the input or output stream when the modulator is not IDLE. + * + * @section Thread Safety + * + * Internally, the modulator is thread-safe. It is running with a background thread + * reading from and writing to thread-safe queues. Externally, the modulator expects + * that all API calls made synchronously as if from a single thread of control. + * + * @section Convertion Functions + * + * There are two public static conversion functions provided to support conversion of + * the output bitstream into either a symbol stream or into a 48ksps baseband stream. + */ +struct M17Modulator +{ +public: + using bitstream_queue_t = queue; // 1 frame's worth of data, 48 bytes, 192 symbols, 384 bits. + using audio_queue_t = queue; // 1 frame's worth of data. + using symbols_t = std::array; // One frame of symbols. + using baseband_t = std::array; // One frame of baseband data @ 48ksps + using bitstream_t = std::array; // M17 frame of bits (in bytes). + + enum class State {INACTIVE, IDLE, PREAMBLE, LINK_SETUP, ACTIVE, END_OF_STREAM}; + +private: + using lsf_t = std::array; // Link setup frame bytes. + + using lich_segment_t = std::array; // Golay-encoded LICH. + using lich_t = std::array; // All LICH segments. + using audio_frame_t = std::array; + using codec_frame_t = std::array; + using payload_t = std::array; // Bytes in the payload of a data frame. + using frame_t = std::array; // M17 frame (without sync word). + + static constexpr std::array SYNC_WORD = {0x32, 0x43}; + static constexpr std::array LSF_SYNC_WORD = {0x55, 0xF7}; + static constexpr std::array DATA_SYNC_WORD = {0xFF, 0x5D}; + + + std::shared_ptr audio_queue_; // Input queue. + std::shared_ptr bitstream_queue_; // Output queue. + std::atomic state_; + struct CODEC2* codec2_ = nullptr; + M17ByteRandomizer<46> randomizer_; + PolynomialInterleaver<45, 92, 368> interleaver_; + CRC16<0x5935, 0xFFFF> crc_; + LinkSetupFrame::encoded_call_t source_; + LinkSetupFrame::encoded_call_t dest_; + + static LinkSetupFrame::encoded_call_t encode_callsign(std::string callsign) + { + LinkSetupFrame::encoded_call_t encoded_call = {0xff,0xff,0xff,0xff,0xff,0xff}; + + if (callsign.empty() || callsign.size() > 9) return encoded_call; + + mobilinkd::LinkSetupFrame::call_t call; + call.fill(0); + std::copy(callsign.begin(), callsign.end(), call.begin()); + encoded_call = LinkSetupFrame::encode_callsign(call); + return encoded_call; + } + + static constexpr int8_t bits_to_symbol(uint8_t bits) + { + switch (bits) + { + case 0: return 1; + case 1: return 3; + case 2: return -1; + case 3: return -3; + } + return 0; + } + + template + static std::array bits_to_symbols(const std::array& bits) + { + std::array result; + size_t index = 0; + for (size_t i = 0; i != N; i += 2) + { + result[index++] = bits_to_symbol((bits[i] << 1) | bits[i + 1]); + } + return result; + } + + void output_frame(std::array sync_word, const frame_t& frame) + { + for (auto c : sync_word) bitstream_queue_->put(c); + for (auto c : frame) bitstream_queue_->put(c); + } + + void send_preamble() + { + // Preamble is simple... bytes -> symbols. + std::array preamble_bytes; + preamble_bytes.fill(0x77); + for (auto c : preamble_bytes) bitstream_queue_->put(c); + } + + template + static std::array conv_encode(std::array data) + { + std::array result; + + uint8_t bit_index = 0; + uint8_t byte_index = 0; + uint8_t tmp = 0; + + uint32_t memory = 0; + for (auto b : data) + { + for (size_t i = 0; i != 8; ++i) + { + uint32_t x = (b & 0x80) >> 7; + b <<= 1; + memory = update_memory<4>(memory, x); + tmp = (tmp << 1) | convolve_bit(031, memory); + tmp = (tmp << 1) | convolve_bit(027, memory); + bit_index += 2; + if (bit_index == 8) + { + bit_index = 0; + result[byte_index++] = tmp; + tmp = 0; + } + } + } + + // Flush the encoder. + for (size_t i = 0; i != 4; ++i) + { + memory = update_memory<4>(memory, 0); + tmp = (tmp << 1) | convolve_bit(031, memory); + tmp = (tmp << 1) | convolve_bit(027, memory); + bit_index += 2; + if (bit_index == 8) + { + bit_index = 0; + result[byte_index++] = tmp; + tmp = 0; + } + } + + // Frame may not end on a byte boundary. + if (bit_index != 0) + { + while (bit_index++ != 8) tmp <<= 1; + result[byte_index] = tmp; + } + + return result; + } + + /** + * Encode each LSF segment into a Golay-encoded LICH segment bitstream. + */ + lich_segment_t make_lich_segment(std::array segment, uint8_t segment_number) + { + lich_segment_t result; + uint16_t tmp; + uint32_t encoded; + + tmp = segment[0] << 4 | ((segment[1] >> 4) & 0x0F); + encoded = mobilinkd::Golay24::encode24(tmp); + for (size_t i = 0; i != 24; ++i) + { + assign_bit_index(result, i, (encoded & (1 << 23)) != 0); + encoded <<= 1; + } + + tmp = ((segment[1] & 0x0F) << 8) | segment[2]; + encoded = mobilinkd::Golay24::encode24(tmp); + for (size_t i = 24; i != 48; ++i) + { + assign_bit_index(result, i, (encoded & (1 << 23)) != 0); + encoded <<= 1; + } + + tmp = segment[3] << 4 | ((segment[4] >> 4) & 0x0F); + encoded = mobilinkd::Golay24::encode24(tmp); + for (size_t i = 48; i != 72; ++i) + { + assign_bit_index(result, i, (encoded & (1 << 23)) != 0); + encoded <<= 1; + } + + tmp = ((segment[4] & 0x0F) << 8) | (segment_number << 5); + encoded = mobilinkd::Golay24::encode24(tmp); + for (size_t i = 72; i != 96; ++i) + { + assign_bit_index(result, i, (encoded & (1 << 23)) != 0); + encoded <<= 1; + } + + return result; + } + + /** + * Construct the link setup frame and split into LICH segments. Output the + * link setup frame and return the LICH segments to the caller. + */ + void send_link_setup(lich_t& lich) + { + using namespace mobilinkd; + + lsf_t lsf; + lsf.fill(0); + + auto rit = std::copy(source_.begin(), source_.end(), lsf.begin()); + std::copy(dest_.begin(), dest_.end(), rit); + lsf[12] = 0; + lsf[13] = 5; + + crc_.reset(); + for (size_t i = 0; i != 28; ++i) + { + crc_(lsf[i]); + } + auto checksum = crc_.get_bytes(); + lsf[28] = checksum[0]; + lsf[29] = checksum[1]; + + // Build LICH segments + for (size_t i = 0; i != lich.size(); ++i) + { + std::array segment; + std::copy(lsf.begin() + i * 5, lsf.begin() + (i + 1) * 5, segment.begin()); + auto lich_segment = make_lich_segment(segment, i); + std::copy(lich_segment.begin(), lich_segment.end(), lich[i].begin()); + } + + auto encoded = conv_encode(lsf); + + std::array punctured; + auto size = puncture_bytes(encoded, punctured, P1); + assert(size == 368); + + interleaver_.interleave(punctured); + randomizer_(punctured); + output_frame(LSF_SYNC_WORD, punctured); + } + + /** + * Append the LICH and Convolutionally encoded payload, interleave and randomize + * the frame bits, and output the frame. + */ + void send_audio_frame(const lich_segment_t& lich, const payload_t& data) + { + using namespace mobilinkd; + + std::array temp; + auto it = std::copy(lich.begin(), lich.end(), temp.begin()); + std::copy(data.begin(), data.end(), it); + + interleaver_.interleave(temp); + randomizer_(temp); + output_frame(DATA_SYNC_WORD, temp); + } + + /** + * Assemble the audio frame payload by appending the frame number, encoded audio, + * and CRC, then convolutionally coding and puncturing the data. + */ + payload_t make_payload(uint16_t frame_number, const codec_frame_t& payload) + { + std::array data; // FN, Audio, CRC = 2 + 16 + 2; + data[0] = uint8_t((frame_number >> 8) & 0xFF); + data[1] = uint8_t(frame_number & 0xFF); + std::copy(payload.begin(), payload.end(), data.begin() + 2); + + crc_.reset(); + for (size_t i = 0; i != 18; ++i) crc_(data[i]); + auto checksum = crc_.get_bytes(); + data[18] = checksum[0]; + data[19] = checksum[1]; + + auto encoded = conv_encode(data); + + payload_t punctured; + auto size = puncture_bytes(encoded, punctured, mobilinkd::P2); + assert(size == 272); + return punctured; + } + + /** + * Encode 2 frames of data. Caller must ensure that the audio is + * padded with 0s if the incoming data is incomplete. + */ + codec_frame_t encode_audio(const audio_frame_t& audio) + { + codec_frame_t result; + codec2_encode(codec2_, &result[0], const_cast(&audio[0])); + codec2_encode(codec2_, &result[8], const_cast(&audio[160])); + return result; + } + + /** + * Send the audio frame. Encodes the audio, assembles the audio frame, and + * outputs the frame on the queue. + */ + void send_audio(const lich_segment_t& lich, uint16_t frame_number, const audio_frame_t& audio) + { + auto encoded_audio = encode_audio(audio); + auto payload = make_payload(frame_number, encoded_audio); + send_audio_frame(lich, payload); + } + + /** + * Modulator state machine. Controls state transitions, ensuring that the + * M17 stream is sent and terminated appropriately. + */ + void modulate() + { + using namespace std::chrono_literals; + using clock = std::chrono::steady_clock; + + state_ = State::IDLE; + codec2_ = ::codec2_create(CODEC2_MODE_3200); + + lich_t lich; + size_t index = 0; + uint16_t frame_number = 0; + uint8_t lich_segment = 0; + audio_frame_t audio; + auto current = clock::now(); + + audio.fill(0); + + while (audio_queue_->is_open() && bitstream_queue_->is_open()) + { + int16_t sample; + if (!(audio_queue_->get(sample, 5s))) sample = 0; // May be closed. + if (!(audio_queue_->is_open())) + { + std::clog << "audio output queue closed" << std::endl; + break; + } + switch (state_) + { + case State::IDLE: + break; + case State::PREAMBLE: + send_preamble(); + state_ = State::LINK_SETUP; + break; + case State::LINK_SETUP: + send_link_setup(lich); + index = 0; + frame_number = 0; + lich_segment = 0; + state_ = State::ACTIVE; + current = clock::now(); + break; + case State::ACTIVE: + audio[index++] = sample; + if (index == audio.size()) + { + auto now = clock::now(); + if (now - current > 40ms) + { + std::clog << "WARNING: packet time exceeded" << std::endl; + } + current = now; + index = 0; + send_audio(lich[lich_segment++], frame_number++, audio); + if (frame_number == 0x8000) frame_number = 0; + if (lich_segment == lich.size()) lich_segment = 0; + audio.fill(0); + } + break; + case State::END_OF_STREAM: + audio[index++] = sample; + send_audio(lich[lich_segment++], frame_number++, audio); + audio.fill(0); + state_ = State::IDLE; + break; + default: + assert(false && "Invalid state"); + } + } + + ::codec2_destroy(codec2_); + codec2_ = nullptr; + + if (state_ != State::IDLE) throw std::logic_error("queue closed when not IDLE"); + + state_ = State::INACTIVE; + } + +public: + + M17Modulator(const std::string& source, const std::string& dest = "") : + source_(encode_callsign(source)), + dest_(encode_callsign(dest)) + { + state_.store(State::INACTIVE); + } + + /** + * Set the source identifier (callsign) for the transmitter. + */ + void source(const std::string& callsign) + { + source_ = encode_callsign(callsign); + } + + /** + * Set the destination identifier for the transmitter. A blank value is + * interpreted as the broadcast address. This is the default. + */ + void dest(const std::string& callsign) + { + dest_ = encode_callsign(callsign); + } + + /** + * Start the modulator. This starts a background thread and returns once the thread + * has started and changed the state to IDLE. + * + * @pre state is INACTIVE. + * + * @param input is a shared pointer to the audio input queue. + * @param output is a shared pointer to the symbol output queue. + * @return a future which is used to return error information to the caller. + */ + std::future run(const std::shared_ptr& input, const std::shared_ptr& output) + { + using namespace std::chrono_literals; + + assert(state_ == State::INACTIVE); + + audio_queue_ = input; + bitstream_queue_ = output; + + auto result = std::async(std::launch::async, [this](){ + this->modulate(); + }); + + // Wait until thread is active. + while (state_ != State::IDLE) std::this_thread::yield(); + + return result; + } + + /** + * Activate the modulator. This causes the modulator to transition from IDLE to + * ACTIVE. If the modulator is already ACTIVE, no action is taken. If the modulator + * is not IDLE, return is delayed until the modulator becomes IDLE (which may take + * up to 120ms), at which time the modulator is returned to the ACTIVE state. + * Otherwise the modulator immediately transistions from IDLE to ACTIVE. This will + * cause the preamble and link setup frames to be emitted. + * + * @pre run must have been called. + * @pre the input queue must be open. + * @pre the output queue must be open. + */ + void ptt_on() + { + using namespace std::chrono_literals; + + assert(state_ != State::INACTIVE); + assert(audio_queue_ && audio_queue_->is_open()); + assert(bitstream_queue_ && bitstream_queue_->is_open()); + + if (state_ == State::ACTIVE) return; + while (state_ != State::IDLE && state_ != State::INACTIVE) std::this_thread::sleep_for(1ms); + assert(state_ == State::IDLE); // Precondition violated -- one of the queues was closed. + state_ = State::PREAMBLE; + } + + /** + * Stop the modulator. + * + * @pre ptt_on() was called and the modulator is in PREAMBLE, LINK_SETUP, or ACTIVE state. + */ + void ptt_off() + { + using namespace std::chrono_literals; + + assert(state_ == State::PREAMBLE | state_ == State::LINK_SETUP | state_ == State::ACTIVE); + + // State must become active before we release PTT to ensure preamble and LSF are sent. + while (state_ != State::ACTIVE && state_ != State::INACTIVE) std::this_thread::sleep_for(1ms); + assert(state_ == State::ACTIVE); // Precondition violated -- one of the queues was closed. + state_ = State::END_OF_STREAM; + } + + void wait_until_idle() + { + using namespace std::chrono_literals; + + while (state_ != State::IDLE && state_ != State::INACTIVE) std::this_thread::sleep_for(1ms); + } + + void wait_until_inactive() + { + using namespace std::chrono_literals; + + while (state_ != State::INACTIVE) std::this_thread::sleep_for(1ms); + } + + State state() const { return state_; } + + template + static std::array bytes_to_symbols(const std::array& bytes) + { + std::array result; + size_t index = 0; + for (auto b : bytes) + { + for (size_t i = 0; i != 4; ++i) + { + result[index++] = bits_to_symbol(b >> 6); + b <<= 2; + } + } + return result; + } + + static baseband_t symbols_to_baseband(const symbols_t& symbols) + { + // Generated using scikit-commpy + static const auto rrc_taps = std::array{ + -0.009265784007800534, -0.006136551625729697, -0.001125978562075172, 0.004891777252042491, + 0.01071805138282269, 0.01505751553351295, 0.01679337935001369, 0.015256245142156299, + 0.01042830577908502, 0.003031522725559901, -0.0055333532968188165, -0.013403099825723372, + -0.018598682349642525, -0.01944761739590459, -0.015005271935951746, -0.0053887880354343935, + 0.008056525910253532, 0.022816244158307273, 0.035513467692208076, 0.04244131815783876, + 0.04025481153629372, 0.02671818654865632, 0.0013810216516704976, -0.03394615682795165, + -0.07502635967975885, -0.11540977897637611, -0.14703962203941534, -0.16119995609538576, + -0.14969512896336504, -0.10610329539459686, -0.026921412469634916, 0.08757875030779196, + 0.23293327870303457, 0.4006012210123992, 0.5786324696325503, 0.7528286479934068, + 0.908262741447522, 1.0309661131633199, 1.1095611856548013, 1.1366197723675815, + 1.1095611856548013, 1.0309661131633199, 0.908262741447522, 0.7528286479934068, + 0.5786324696325503, 0.4006012210123992, 0.23293327870303457, 0.08757875030779196, + -0.026921412469634916, -0.10610329539459686, -0.14969512896336504, -0.16119995609538576, + -0.14703962203941534, -0.11540977897637611, -0.07502635967975885, -0.03394615682795165, + 0.0013810216516704976, 0.02671818654865632, 0.04025481153629372, 0.04244131815783876, + 0.035513467692208076, 0.022816244158307273, 0.008056525910253532, -0.0053887880354343935, + -0.015005271935951746, -0.01944761739590459, -0.018598682349642525, -0.013403099825723372, + -0.0055333532968188165, 0.003031522725559901, 0.01042830577908502, 0.015256245142156299, + 0.01679337935001369, 0.01505751553351295, 0.01071805138282269, 0.004891777252042491, + -0.001125978562075172, -0.006136551625729697, -0.009265784007800534 + }; + static BaseFirFilter::value> rrc = makeFirFilter(rrc_taps); + + std::array baseband; + baseband.fill(0); + for (size_t i = 0; i != symbols.size(); ++i) + { + baseband[i * 10] = symbols[i]; + } + + for (auto& b : baseband) + { + b = rrc(b) * 25; + } + return baseband; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Randomizer.h b/plugins/channelrx/demodm17/m17/M17Randomizer.h new file mode 100644 index 000000000..f3d9c002e --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Randomizer.h @@ -0,0 +1,79 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +namespace mobilinkd +{ + +namespace detail +{ + +// M17 randomization matrix. +static const std::array DC = std::array{ + 0xd6, 0xb5, 0xe2, 0x30, 0x82, 0xFF, 0x84, 0x62, + 0xba, 0x4e, 0x96, 0x90, 0xd8, 0x98, 0xdd, 0x5d, + 0x0c, 0xc8, 0x52, 0x43, 0x91, 0x1d, 0xf8, 0x6e, + 0x68, 0x2F, 0x35, 0xda, 0x14, 0xea, 0xcd, 0x76, + 0x19, 0x8d, 0xd5, 0x80, 0xd1, 0x33, 0x87, 0x13, + 0x57, 0x18, 0x2d, 0x29, 0x78, 0xc3}; +} + +template +struct M17Randomizer +{ + std::array dc_; + + M17Randomizer() + { + size_t i = 0; + for (auto b : detail::DC) + { + for (size_t j = 0; j != 8; ++j) + { + dc_[i++] = (b >> (7 - j)) & 1 ? -1 : 1; + } + } + } + + // Randomize and derandomize are the same operation. + void operator()(std::array& frame) + { + for (size_t i = 0; i != N; ++i) + { + frame[i] *= dc_[i]; + } + } + + void randomize(std::array& frame) + { + for (size_t i = 0; i != N; ++i) + { + frame[i] ^= (dc_[i] == -1); + } + } + +}; + +template +struct M17ByteRandomizer +{ + // Randomize and derandomize are the same operation. + void operator()(std::array& frame) + { + for (size_t i = 0; i != N; ++i) + { + for (size_t j = 8; j != 0; --j) + { + uint8_t mask = 1 << (j - 1); + frame[i] = (frame[i] & ~mask) | ((frame[i] & mask) ^ (detail::DC[i] & mask)); + } + } + } +}; + + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/M17Synchronizer.h b/plugins/channelrx/demodm17/m17/M17Synchronizer.h new file mode 100644 index 000000000..d28da5837 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/M17Synchronizer.h @@ -0,0 +1,36 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include + +#include "Util.h" + +namespace mobilinkd +{ + +struct M17Synchronizer +{ + uint16_t expected_; + int allowable_errors_; + uint16_t buffer_ = 0; + + M17Synchronizer(uint16_t word = 0x3243, int bit_errors = 1) + : expected_(word), allowable_errors_(bit_errors) + {} + + bool operator()(int bits) + { + // Add one symbol (2 bits) of data to the synchronizer. + // Returns true when a sync word has been detected. + + buffer_ = ((buffer_ << 2) | bits) & 0xFFFF; + auto tmp = buffer_ ^ expected_; + return popcount(tmp) <= allowable_errors_; + } + + void reset() { buffer_ = 0; } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/PhaseEstimator.h b/plugins/channelrx/demodm17/m17/PhaseEstimator.h new file mode 100644 index 000000000..9e50a05e7 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/PhaseEstimator.h @@ -0,0 +1,51 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include + +namespace mobilinkd +{ + +/** + * Estimate the phase of a sample by estimating the + * tangent of the sample point. This is done by computing + * the magnitude difference of the previous and following + * samples. We do not correct for 0-crossing errors because + * these errors have not affected the performance of clock + * recovery. + */ +template +struct PhaseEstimator +{ + using float_type = FloatType; + using samples_t = std::array; // 3 samples in length + + float_type dx_; + + PhaseEstimator(FloatType sample_rate, FloatType symbol_rate) + : dx_(2.0 * symbol_rate / sample_rate) + {} + + /** + * This performs a rolling estimate of the phase. + * + * @param samples are three samples centered around the current sample point + * (t-1, t, t+1). + */ + float_type operator()(const samples_t& samples) + { + assert(dx_ > 0.0); + + auto ratio = ((samples.at(2) - samples.at(0)) / 3.0) / dx_; + // Clamp +/-5. + ratio = std::min(FloatType(5.0), ratio); + ratio = std::max(FloatType(-5.0), ratio); + + return ratio; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/PolynomialInterleaver.h b/plugins/channelrx/demodm17/m17/PolynomialInterleaver.h new file mode 100644 index 000000000..57575b551 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/PolynomialInterleaver.h @@ -0,0 +1,73 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "Util.h" + +#include +#include + +namespace mobilinkd +{ + +template +struct PolynomialInterleaver +{ + using buffer_t = std::array; + using bytes_t = std::array; + + alignas(16) buffer_t buffer_; + + size_t index(size_t i) + { + return ((F1 * i) + (F2 * i * i)) % K; + } + + void interleave(buffer_t& data) + { + buffer_.fill(0); + + for (size_t i = 0; i != K; ++i) + buffer_[index(i)] = data[i]; + + std::copy(std::begin(buffer_), std::end(buffer_), std::begin(data)); + } + + void interleave(bytes_t& data) + { + bytes_t buffer; + buffer.fill(0); + for (size_t i = 0; i != K; ++i) + { + assign_bit_index(buffer, index(i), get_bit_index(data, i)); + } + std::copy(buffer.begin(), buffer.end(), data.begin()); + } + + void deinterleave(buffer_t& frame) + { + buffer_.fill(0); + + for (size_t i = 0; i != K; ++i) + { + auto idx = index(i); + buffer_[i] = frame[idx]; + } + + std::copy(buffer_.begin(), buffer_.end(), frame.begin()); + } + + void deinterleave(bytes_t& data) + { + bytes_t buffer; + buffer.fill(0); + for (size_t i = 0; i != K; ++i) + { + assign_bit_index(buffer, i, get_bit_index(data, index(i))); + } + std::copy(buffer.begin(), buffer.end(), data.begin()); + } + +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/SlidingDFT.h b/plugins/channelrx/demodm17/m17/SlidingDFT.h new file mode 100644 index 000000000..294d88f2b --- /dev/null +++ b/plugins/channelrx/demodm17/m17/SlidingDFT.h @@ -0,0 +1,135 @@ +// 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 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::array samples_; + ComplexType result_{0,0}; + size_t index_ = 0; + size_t prev_index_ = N - 1; + +public: + SlidingDFT() + { + samples_.fill(0); + coeff_ = std::exp(-ComplexType{0, 1} * pi2 * kth); + } + + ComplexType operator()(FloatType sample) + { + auto index = index_; + index_ += 1; + if (index_ == N) index_ = 0; + + FloatType delta = sample - samples_[index]; + ComplexType result = (result_ + delta) * coeff_; + result_ = result * FloatType(0.999999999999999); + samples_[index] = sample; + prev_index_ = index; + 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 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) + { + ComplexType j = ComplexType{0, 1}; + 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) + { + auto index = index_; + index_ += 1; + if (index_ == N) index_ = 0; + + FloatType delta = sample - samples_[index]; + + for (size_t i = 0; i != K; ++i) + { + result_[i] = (result_[i] + delta) * coeff_[i]; + } + samples_[index] = sample; + return result_; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/SymbolEvm.h b/plugins/channelrx/demodm17/m17/SymbolEvm.h new file mode 100644 index 000000000..78b87e04a --- /dev/null +++ b/plugins/channelrx/demodm17/m17/SymbolEvm.h @@ -0,0 +1,85 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "IirFilter.h" + +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +template +struct SymbolEvm +{ + using filter_type = BaseIirFilter; + using symbol_t = int; + using result_type = std::tuple; + + filter_type filter_; + FloatType erasure_limit_; + FloatType evm_ = 0.0; + + SymbolEvm(filter_type&& filter, FloatType erasure_limit = 0.0) : + filter_(std::forward(filter)), + erasure_limit_(erasure_limit) + {} + + FloatType evm() const { return evm_; } + + /** + * Decode a normalized sample into a symbol. Symbols + * are decoded into +3, +1, -1, -3. If an erasure limit + * is set, symbols outside this limit are 'erased' and + * returned as 0. + */ + result_type operator()(FloatType sample) + { + symbol_t symbol; + FloatType evm; + + sample = std::min(3.0, std::max(-3.0, sample)); + + if (sample > 2) + { + symbol = 3; + evm = (sample - 3) * 0.333333; + } + else if (sample > 0) + { + symbol = 1; + evm = sample - 1; + } + else if (sample >= -2) + { + symbol = -1; + evm = sample + 1; + } + else + { + symbol = -3; + evm = (sample + 3) * 0.333333; + } + + if (erasure_limit_ and (abs(evm) > *erasure_limit_)) symbol = 0; + + evm_ = filter_(evm); + + return std::make_tuple(symbol, evm); + } +}; + +template +SymbolEvm makeSymbolEvm( + BaseIirFilter&& filter, + FloatType erasure_limit = 0.0f +) +{ + return std::move(SymbolEvm(std::move(filter), erasure_limit)); +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Trellis.h b/plugins/channelrx/demodm17/m17/Trellis.h new file mode 100644 index 000000000..5488ac884 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Trellis.h @@ -0,0 +1,138 @@ +// Copyright 2020-2021 Mobilinkd LLC. + +// make CXXFLAGS="$(pkg-config --cflags gtest) $(pkg-config --libs gtest) -I. -O3" tests/TrellisTest + +#pragma once + +#include "Util.h" +#include "Convolution.h" + +#include +#include +#include + +namespace mobilinkd +{ + +/// Puncture matrix for LSF +constexpr auto P1 = std::array{ + 1, + 1, 0, 1, 1, // M1 + 1, 0, 1, 1, // M2 + 1, 0, 1, 1, // M3 + 1, 0, 1, 1, // M4 + 1, 0, 1, 1, // M5 + 1, 0, 1, 1, // M6 + 1, 0, 1, 1, // M7 + 1, 0, 1, 1, // M8 + 1, 0, 1, 1, // M9 + 1, 0, 1, 1, // M10 + 1, 0, 1, 1, // M10 + 1, 0, 1, 1, // M12 + 1, 0, 1, 1, // M13 + 1, 0, 1, 1, // M14 + 1, 0, 1, 1 // M15 +}; + +/// Puncture matrix for audio frames. Rate 6/11. +constexpr auto P2 = std::array{ + 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 0}; + + +/// Puncture matrix for packet frames (7/8). +constexpr auto P3 = std::array{ + 1, 1, 1, 1, + 1, 1, 1, 0}; + +/** + * Convert an integer value to an array of bits, with the + * high-bit at index 0. + * + * At anything beyond -O0, the array is constructed at compile time. + */ +template +constexpr std::array toBitArray(int8_t value) +{ + std::array result{}; + for (size_t i = 0; i != N; ++i) + { + result[N - (i + 1)] = (value & 1); + value >>= 1; + } + return result; +} + +template +struct NextStateTable +{ + using nextStateTable_t = std::array, N>; + + nextStateTable_t nextStateTable = makeNextStateTable(); + + static constexpr nextStateTable_t makeNextStateTable() + { + return nextStateTable_t(); + } +}; + +template +struct OutputTable +{ + +}; + +/** + * Compute a cost table for a Trellis of size K, for input n of N, + * and LLR size of LLR bits + 1. (i.e. LLR = 1 allows 2 bits to + * represent -1, 0, +1). + */ +template +struct CostTable +{ + static constexpr int8_t Price = 1 << LLR; + static constexpr size_t InputValues = 1 << N; + using cost_table_t = std::array, K>; + + template + static constexpr cost_table_t makeCostTable(const Trellis_& trellis) + { + cost_table_t result; + for (size_t i = 0; i != K; ++i) + { + for (size_t j = 0; j != InputValues; ++j) + { + + } + } + } +}; + + +/** + * Only valid for a k=1 (1:n) convolutional coder. + */ +template +struct Trellis +{ + static constexpr size_t K = K_; // Memory depth of convolution. + static constexpr size_t k = 1; // Number of bits per input symbol. + static constexpr size_t n = n_; // Number of coefficients / output bits. + static constexpr size_t NumStates = (1 << K); // Number of states in the convolutional coder. + + using polynomials_t = std::array; + + polynomials_t polynomials; + + Trellis(polynomials_t polys) + : polynomials(polys) + {} +}; + +template +constexpr Trellis makeTrellis(std::array polys) +{ + return Trellis(polys); +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Util.h b/plugins/channelrx/demodm17/m17/Util.h new file mode 100644 index 000000000..600be204c --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Util.h @@ -0,0 +1,428 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +namespace mobilinkd +{ + +// The make_bitset stuff only works as expected in GCC10 and later. + +namespace detail { + +template +constexpr std::bitset make_bitset(std::index_sequence, Tuple&& tuple) +{ + constexpr auto size = sizeof...(Is); + std::bitset result; + using expand = int[]; + for (size_t i = 0; i != size; ++i) + { + void(expand {0, result[Is] = std::get(tuple)...}); + } + return result; +} + +/** + * This is the max value for the LLR based on size N. + */ +template +constexpr size_t llr_limit() +{ + return (1 << (N - 1)) - 1; +} + +/** + * There are (2^(N-1)-1) elements (E) per segment (e.g. N=4, E=7; N=3, E=3). + * These contain the LLR values 1..E. There are 6 segments in the LLR map: + * 1. (-Inf,-2] + * 2. (-2, -1] + * 3. (-1, 0] + * 4. (0, 1] + * 5. (1, 2] + * 6. (2, Inf) + * + * Note the slight asymmetry. This is OK as we are dealing with floats and + * it only matters to an epsilon of the float type. + */ +template +constexpr size_t llr_size() +{ + return llr_limit() * 6 + 1; +} + +template +constexpr std::array>, llr_size()> make_llr_map() +{ + constexpr size_t size = llr_size(); + std::array>, size> result; + + constexpr int8_t limit = llr_limit(); + constexpr FloatType inc = 1.0 / FloatType(limit); + int8_t i = limit; + int8_t j = limit; + + // Output must be ordered by k, ascending. + FloatType k = -3.0 + inc; + for (size_t index = 0; index != size; ++index) + { + auto& a = result[index]; + std::get<0>(a) = k; + std::get<0>(std::get<1>(a)) = i; + std::get<1>(std::get<1>(a)) = j; + + if (k + 1.0 < 0) + { + j--; + if (j == 0) j = -1; + if (j < -limit) j = -limit; + } + else if (k - 1.0 < 0) + { + i--; + if (i == 0) i = -1; + if (i < -limit) i = -limit; + } + else + { + j++; + if (j == 0) j = 1; + if (j > limit) j = limit; + } + k += inc; + } + return result; +} + +} + +template +constexpr auto make_bitset(Bools&&...bools) +{ + return detail::make_bitset(std::make_index_sequence(), + std::make_tuple(bool(bools)...)); +} + +inline int from_4fsk(int symbol) +{ + // Convert a 4-FSK symbol to a pair of bits. + switch (symbol) + { + case 1: return 0; + case 3: return 1; + case -1: return 2; + case -3: return 3; + default: abort(); + } +} + +template +auto llr(FloatType sample) +{ + static auto symbol_map = detail::make_llr_map(); + static constexpr FloatType MAX_VALUE = 3.0; + static constexpr FloatType MIN_VALUE = -3.0; + + FloatType s = std::min(MAX_VALUE, std::max(MIN_VALUE, sample)); + + auto it = std::lower_bound(symbol_map.begin(), symbol_map.end(), s, + [](std::tuple> const& e, FloatType s){ + return std::get<0>(e) < s; + }); + + if (it == symbol_map.end()) return std::get<1>(*symbol_map.rbegin()); + + return std::get<1>(*it); +} + +template +auto depunctured(std::array puncture_matrix, std::array in) +{ + static_assert(M % N == 0); + std::array result; + size_t index = 0; + size_t pindex = 0; + for (size_t i = 0; i != M; ++i) + { + if (!puncture_matrix[pindex++]) + { + result[i] = 0; + } + else + { + result[i] = in[index++]; + } + if (pindex == N) pindex = 0; + } + return result; +} + +template +size_t depuncture(const std::array& in, + std::array& out, const std::array& p) +{ + size_t index = 0; + size_t pindex = 0; + size_t bit_count = 0; + for (size_t i = 0; i != OUT && index < IN; ++i) + { + if (!p[pindex++]) + { + out[i] = 0; + bit_count++; + } + else + { + out[i] = in[index++]; + } + if (pindex == P) pindex = 0; + } + return bit_count; +} + + +template +size_t puncture(const std::array& in, + std::array& out, const std::array& p) +{ + size_t index = 0; + size_t pindex = 0; + size_t bit_count = 0; + for (size_t i = 0; i != IN && index != OUT; ++i) + { + if (p[pindex++]) + { + out[index++] = in[i]; + bit_count++; + } + + if (pindex == P) pindex = 0; + } + return bit_count; +} + +template +constexpr bool get_bit_index(const std::array& input, size_t index) +{ + auto byte_index = index >> 3; + assert(byte_index < N); + auto bit_index = 7 - (index & 7); + + return (input[byte_index] & (1 << bit_index)) >> bit_index; +} + +template +void set_bit_index(std::array& input, size_t index) +{ + auto byte_index = index >> 3; + assert(byte_index < N); + auto bit_index = 7 - (index & 7); + input[byte_index] |= (1 << bit_index); +} + +template +void reset_bit_index(std::array& input, size_t index) +{ + auto byte_index = index >> 3; + assert(byte_index < N); + auto bit_index = 7 - (index & 7); + input[byte_index] &= ~(1 << bit_index); +} + +template +void assign_bit_index(std::array& input, size_t index, bool value) +{ + if (value) set_bit_index(input, index); + else reset_bit_index(input, index); +} + + +template +size_t puncture_bytes(const std::array& in, + std::array& out, const std::array& p) +{ + size_t index = 0; + size_t pindex = 0; + size_t bit_count = 0; + for (size_t i = 0; i != IN * 8 && index != OUT * 8; ++i) + { + if (p[pindex++]) + { + assign_bit_index(out, index++, get_bit_index(in, i)); + bit_count++; + } + + if (pindex == P) pindex = 0; + } + return bit_count; +} + +/** + * Sign-extend an n-bit value to a specific signed integer type. + */ +template +constexpr T to_int(uint8_t v) +{ + constexpr auto MAX_LOCAL_INPUT = (1 << (n - 1)); + constexpr auto NEGATIVE_OFFSET = std::numeric_limits::type>::max() - (MAX_LOCAL_INPUT - 1); + T r = v & (1 << (n - 1)) ? NEGATIVE_OFFSET : 0; + return r + (v & (MAX_LOCAL_INPUT - 1)); +} + +template +constexpr auto to_byte_array(std::array in) +{ + std::array out{}; + out.fill(0); + size_t i = 0; + size_t b = 0; + for (auto c : in) + { + out[i] |= (c << (7 - b)); + if (++b == 8) + { + ++i; + b = 0; + } + } + return out; +} + +template +constexpr void to_byte_array(std::array in, std::array& out) +{ + size_t i = 0; + size_t b = 0; + uint8_t tmp = 0; + for (auto c : in) + { + tmp |= (c << (7 - b)); + if (++b == 8) + { + out[i] = tmp; + tmp = 0; + ++i; + b = 0; + } + } + if (i < out.size()) out[i] = tmp; +} + +struct PRBS9 +{ + static constexpr uint16_t MASK = 0x1FF; + static constexpr uint8_t TAP_1 = 8; // Bit 9 + static constexpr uint8_t TAP_2 = 4; // Bit 5 + static constexpr uint8_t LOCK_COUNT = 18; // 18 consecutive good bits. + static constexpr uint8_t UNLOCK_COUNT = 25; // bad bits in history required to unlock. + + uint16_t state = 1; + bool synced = false; + uint8_t sync_count = 0; + uint32_t bit_count = 0; + uint32_t err_count = 0; + std::array history; + size_t hist_count = 0; + size_t hist_pos = 0; + + void count_errors(bool error) + { + bit_count += 1; + hist_count -= (history[hist_pos >> 3] & (1 << (hist_pos & 7))) != 0; + if (error) { + err_count += 1; + hist_count += 1; + history[hist_pos >> 3] |= (1 << (hist_pos & 7)); + if (hist_count >= UNLOCK_COUNT) synced = false; + } else { + history[hist_pos >> 3] &= ~(1 << (hist_pos & 7)); + } + if (++hist_pos == 128) hist_pos = 0; + } + + // PRBS generator. + bool generate() + { + bool result = ((state >> TAP_1) ^ (state >> TAP_2)) & 1; + state = ((state << 1) | result) & MASK; + return result; + } + + // PRBS Syncronizer. Returns 0 if the bit matches the PRBS, otherwise 1. + // When synchronizing the LFSR used in the PRBS, a single bad input bit will + // result in 3 error bits being emitted. + bool synchronize(bool bit) + { + bool result = (bit ^ (state >> TAP_1) ^ (state >> TAP_2)) & 1; + state = ((state << 1) | bit) & MASK; + if (result) { + sync_count = 0; // error + } else { + if (++sync_count == LOCK_COUNT) { + synced = true; + bit_count += LOCK_COUNT; + history.fill(0); + hist_count = 0; + hist_pos = 0; + sync_count = 0; + } + } + return result; + } + + // PRBS validator. Returns 0 if the bit matches the PRBS, otherwise 1. + // The results are only valid when sync() returns true; + bool validate(bool bit) + { + bool result; + if (!synced) { + result = synchronize(bit); + } else { + // PRBS is now free-running. + result = bit ^ generate(); + count_errors(result); + } + return result; + } + + bool sync() const { return synced; } + uint32_t errors() const { assert(synced); return err_count; } + uint32_t bits() const { assert(synced); return bit_count; } + + // Reset the state. + void reset() + { + state = 1; + synced = false; + sync_count = 0; + bit_count = 0; + err_count = 0; + history.fill(0); + hist_count = 0; + hist_pos = 0; + } +}; + +template< class T > +constexpr int popcount( T x ) noexcept +{ + int count = 0; + + while (x) + { + count += x & 1; + x >>= 1; + } + + return count; +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/Viterbi.h b/plugins/channelrx/demodm17/m17/Viterbi.h new file mode 100644 index 000000000..a92d83fc7 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/Viterbi.h @@ -0,0 +1,242 @@ +// Copyright 2020 Mobilinkd LLC. + +#pragma once + +#include "Trellis.h" +#include "Convolution.h" +#include "Util.h" + +#include +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +/** + * Compile-time build of the trellis forward state transitions. + * + * @param is the trellis -- used only for type deduction. + * @return a 2-D array of source, dest, cost. + */ +template +constexpr std::array, (1 << Trellis_::K)> makeNextState(Trellis_) +{ + std::array, (1 << Trellis_::K)> result{}; + for (size_t i = 0; i != (1 << Trellis_::K); ++i) + { + for (size_t j = 0; j != (1 << Trellis_::k); ++j) + { + result[i][j] = static_cast(update_memory(i, j) & ((1 << Trellis_::K) - 1)); + } + } + return result; +} + + +/** + * Compile-time build of the trellis reverse state transitions, for efficient + * reverse traversal during chainback. + * + * @param is the trellis -- used only for type deduction. + * @return a 2-D array of dest, source, cost. + */ +template +constexpr std::array, (1 << Trellis_::K)> makePrevState(Trellis_) +{ + constexpr size_t NumStates = (1 << Trellis_::K); + constexpr size_t HalfStates = NumStates / 2; + + std::array, (1 << Trellis_::K)> result{}; + for (size_t i = 0; i != (1 << Trellis_::K); ++i) + { + size_t k = i >= HalfStates; + for (size_t j = 0; j != (1 << Trellis_::k); ++j) + { + size_t l = update_memory(i, j) & (NumStates - 1); + result[l][k] = i; + } + } + return result; +} + +/** + * Compile-time generation of the trellis path cost for LLR. + * + * @param trellis + * @return + */ +template +constexpr auto makeCost(Trellis_ trellis) +{ + constexpr size_t NumStates = (1 << Trellis_::K); + constexpr size_t NumOutputs = Trellis_::n; + + std::array, NumStates> result{}; + for (uint32_t i = 0; i != NumStates; ++i) + { + for (uint32_t j = 0; j != NumOutputs; ++j) + { + auto bit = convolve_bit(trellis.polynomials[j], i << 1); + result[i][j] = to_int(((bit << 1) - 1) * ((1 << (LLR - 1)) - 1)); + } + } + return result; +} + +/** + * Soft decision Viterbi algorithm based on the trellis and LLR size. + * + */ +template +struct Viterbi +{ + static_assert(LLR_ < 7); // Need to be < 7 to avoid overflow errors. + + static constexpr size_t K = Trellis_::K; + static constexpr size_t k = Trellis_::k; + static constexpr size_t n = Trellis_::n; + static constexpr size_t InputValues = 1 << n; + static constexpr size_t NumStates = (1 << K); + static constexpr int32_t METRIC = ((1 << (LLR_ - 1)) - 1) << 2; + + using metrics_t = std::array; + using cost_t = std::array, NumStates>; + using state_transition_t = std::array, NumStates>; + + metrics_t pathMetrics_{}; + cost_t cost_; + state_transition_t nextState_; + state_transition_t prevState_; + + metrics_t prevMetrics, currMetrics; + + // This is the maximum amount of storage needed for M17. If used for + // other modes, this may need to be increased. This will never overflow + // because of a static assertion in the decode() function. + std::array, 244> history_; + + Viterbi(Trellis_ trellis) + : cost_(makeCost(trellis)) + , nextState_(makeNextState(trellis)) + , prevState_(makePrevState(trellis)) + {} + + void calculate_path_metric( + const std::array& cost0, + const std::array& cost1, + std::bitset& hist, + size_t j + ) { + auto& i0 = nextState_[j][0]; + auto& i1 = nextState_[j][1]; + + auto& c0 = cost0[j]; + auto& c1 = cost1[j]; + + auto& p0 = prevMetrics[j]; + auto& p1 = prevMetrics[j + NumStates / 2]; + + int32_t m0 = p0 + c0; + int32_t m1 = p0 + c1; + int32_t m2 = p1 + c1; + int32_t m3 = p1 + c0; + + bool d0 = m0 > m2; + bool d1 = m1 > m3; + + hist.set(i0, d0); + hist.set(i1, d1); + currMetrics[i0] = d0 ? m2 : m0; + currMetrics[i1] = d1 ? m3 : m1; + } + + /** + * Viterbi soft decoder using LLR inputs where 0 == erasure. + * + * @return path metric for estimating BER. + */ + template + size_t decode(std::array const& in, std::array& out) + { + static_assert(sizeof(history_) >= IN / 2); + + constexpr auto MAX_METRIC = std::numeric_limits::max() / 2; + + prevMetrics.fill(MAX_METRIC); + prevMetrics[0] = 0; // Starting point. + + auto hbegin = history_.begin(); + auto hend = history_.begin() + IN / 2; + + constexpr size_t BUTTERFLY_SIZE = NumStates / 2; + + size_t hindex = 0; + std::array cost0; + std::array cost1; + + for (size_t i = 0; i != IN; i += 2, hindex += 1) + { + int16_t s0 = in[i]; + int16_t s1 = in[i + 1]; + cost0.fill(0); + cost1.fill(0); + + for (size_t j = 0; j != BUTTERFLY_SIZE; ++j) + { + if (s0) // is not erased + { + cost0[j] = std::abs(cost_[j][0] - s0); + cost1[j] = std::abs(cost_[j][0] + s0); + } + if (s1) // is not erased + { + cost0[j] += std::abs(cost_[j][1] - s1); + cost1[j] += std::abs(cost_[j][1] + s1); + } + } + + for (size_t j = 0; j != BUTTERFLY_SIZE; ++j) + { + calculate_path_metric(cost0, cost1, history_[hindex], j); + } + std::swap(currMetrics, prevMetrics); + } + + // Find starting point. Should be 0 for properly flushed CCs. + // However, 0 may not be the path with the fewest errors. + size_t min_element = 0; + int32_t min_cost = prevMetrics[0]; + + for (size_t i = 0; i != NumStates; ++i) + { + if (prevMetrics[i] < min_cost) + { + min_cost = prevMetrics[i]; + min_element = i; + } + } + + size_t cost = std::round(min_cost / float(detail::llr_limit())); + + // Do chainback. + auto oit = std::rbegin(out); + auto hit = std::make_reverse_iterator(hend); // rbegin + auto hrend = std::make_reverse_iterator(hbegin); // rend + size_t next_element = min_element; + size_t index = IN / 2; + while (oit != std::rend(out) && hit != hrend) + { + auto v = (*hit++)[next_element]; + if (index-- <= OUT) *oit++ = next_element & 1; + next_element = prevState_[next_element][v]; + } + + return cost; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/ax25_frame.h b/plugins/channelrx/demodm17/m17/ax25_frame.h new file mode 100644 index 000000000..1bc5a9a46 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/ax25_frame.h @@ -0,0 +1,265 @@ +// Copyright 2012-2021 Rob Riggs +// All rights reserved. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace mobilinkd { + +struct ax25_frame +{ + using repeaters_type = std::vector; + using pid_type = uint8_t; + enum frame_type {UNDEFINED, INFORMATION, SUPERVISORY, UNNUMBERED}; + +private: + + static const std::string::size_type DEST_ADDRESS_POS = 0; + static const std::string::size_type SRC_ADDRESS_POS = 7; + static const std::string::size_type LAST_ADDRESS_POS = 13; + static const std::string::size_type FIRST_REPEATER_POS = 14; + static const std::string::size_type ADDRESS_LENGTH = 7; + + std::string destination_; + std::string source_; + repeaters_type repeaters_; + frame_type type_; + uint8_t raw_type_; + std::string info_; + uint16_t fcs_; + uint16_t crc_; + pid_type pid_; + + static std::string removeAddressExtensionBit(const std::string& address) + { + std::string result = address; + for (size_t i = 0; i != result.size(); i++) + { + result[i] = (uint8_t(result[i]) >> 1); + } + return result; + } + + static int getSSID(const std::string& address) + { + assert(address.size() == ADDRESS_LENGTH); + + return (address[6] & 0x0F); + } + + static std::string appendSSID(const std::string& address, int ssid) + { + std::string result = address; + + if (ssid) + { + result += '-'; + result += std::to_string(ssid); + } + return result; + } + + static bool fixup_address(std::string& address) + { + assert(address.size() == ADDRESS_LENGTH); + + bool result = (address[ADDRESS_LENGTH - 1] & 1) == 0; + + address = removeAddressExtensionBit(address); + + const int ssid = getSSID(address); + + // Remove trailing spaces and SSID. + size_t pos = address.find_first_of(' '); + if (pos == std::string::npos) pos = 6; + address.erase(pos); + + address = appendSSID(address, ssid); + + return result; + } + + static frame_type parse_type(const std::string& frame, size_t pos) + { + uint8_t c(frame[pos]); + switch (c & 0x03) + { + case 0: + return INFORMATION; + case 1: + return SUPERVISORY; + case 2: + return INFORMATION; + default: + return UNNUMBERED; + } + } + + static std::string parse_info(const std::string& frame, size_t pos) + { + std::ostringstream output; + + for (int i = pos; i < ((int) frame.size()) - 2; i++) + { + char c = frame[i]; + if (std::isprint(c)) + { + output << c; + } + else + { + output << "0x" << std::setw(2) + << std::setbase(16) << int(uint8_t(c)) << ' '; + } + } + return output.str(); + } + + static uint16_t parse_fcs(const std::string& frame) + { + size_t checksum_pos = frame.size() - 2; + + uint16_t tmp = + ((uint8_t(frame[checksum_pos + 1]) << 8) | + uint8_t(frame[checksum_pos])); + + uint16_t checksum = 0; + for (size_t i = 1; i != 0x10000; i <<= 1) + { + checksum <<= 1; + checksum |= ((tmp & i) ? 1 : 0); + } + + return checksum; + } + + static std::string parse_destination(const std::string& frame) + { + assert(frame.size() > DEST_ADDRESS_POS + ADDRESS_LENGTH); + return frame.substr(DEST_ADDRESS_POS, ADDRESS_LENGTH); + } + + static std::string parse_source(const std::string& frame) + { + assert(frame.size() > SRC_ADDRESS_POS + ADDRESS_LENGTH); + return frame.substr(SRC_ADDRESS_POS, ADDRESS_LENGTH); + } + + static repeaters_type parse_repeaters(const std::string& frame) + { + repeaters_type result; + std::string::size_type index = FIRST_REPEATER_POS; + bool more = (index + ADDRESS_LENGTH) < frame.length(); + + while (more) + { + std::string repeater = frame.substr(index, ADDRESS_LENGTH); + index += ADDRESS_LENGTH; + more = fixup_address(repeater) + and (index + ADDRESS_LENGTH) < frame.length(); + result.push_back(repeater); + } + + return result; + } + + void parse(const std::string& frame) + { + if (frame.length() < 17) return; + + fcs_ = parse_fcs(frame); + + destination_ = parse_destination(frame); + fixup_address(destination_); + + source_ = parse_source(frame); + bool have_repeaters = fixup_address(source_); + + if (have_repeaters) + { + repeaters_ = parse_repeaters(frame); + } + + size_t index = ADDRESS_LENGTH * (repeaters_.size() + 2); + + if (frame.length() < index + 5) return; + + type_ = parse_type(frame, index); + raw_type_ = uint8_t(frame[index++]); + + if (type_ == UNNUMBERED) pid_ = uint8_t(frame[index++]); + + info_.assign(frame.begin() + index, frame.end() - 2); + } + +public: + + ax25_frame(const std::string& frame) : + destination_(), + source_(), + repeaters_(), + type_(UNDEFINED), + info_(), + fcs_(-1), + crc_(0), + pid_() + { + parse(frame); + } + + std::string destination() const { return destination_; } + + std::string source() const { return source_; } + + repeaters_type repeaters() const { return repeaters_; } + + frame_type type() const { return type_; } + + std::string info() const { return info_; } + + uint16_t fcs() const { return fcs_; } + + uint16_t crc() const { return crc_; } + + pid_type pid() const { return pid_; } +}; + + +void write(std::ostream& os, const ax25_frame& frame) +{ + typedef typename ax25_frame::repeaters_type repeaters_type; + + os << "Dest: " << frame.destination() << std::endl + << "Source: " << frame.source() << std::endl; + + repeaters_type repeaters = frame.repeaters(); + if (!repeaters.empty()) + { + os << "Via: "; + std::copy( + repeaters.begin(), repeaters.end(), + std::ostream_iterator(os, " ")); + os << std::endl; + } + + if (frame.pid()) + { + os << "PID: " << std::setbase(16) << int(frame.pid()) << std::endl; + } + os << "Info: " << std::endl << frame.info() << std::endl; +} + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17/queue.h b/plugins/channelrx/demodm17/m17/queue.h new file mode 100644 index 000000000..a83c83e16 --- /dev/null +++ b/plugins/channelrx/demodm17/m17/queue.h @@ -0,0 +1,251 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace mobilinkd +{ + +/** + * A thread-safe queue + */ +template +class queue +{ +private: + + using mutex_type = std::mutex; + using lock_type = std::unique_lock; + using guard_type = std::lock_guard; + + enum class State {OPEN, CLOSING, CLOSED}; + + std::list queue_; + size_t size_ = 0; + State state_ = State::OPEN; + mutable mutex_type mutex_; + std::condition_variable full_; + std::condition_variable empty_; + + queue(queue&) = delete; + queue& operator=(const queue&) = delete; + +public: + + static constexpr auto forever = std::chrono::seconds::max(); + + /// The data type stored in the queue. + using value_type = T; + + /// A reference to an element stored in the queue. + using reference = value_type&; + + /// A const reference to an element stored in the queue. + using const_reference = value_type const&; + + /// A pointer to an element stored in a Queue. + using pointer = value_type*; + + /// A pointer to an element stored in a Queue. + using const_pointer = const value_type*; + + queue() + {} + + /** + * Get the next item in the queue. + * + * @param[out] val is an object into which the object will be moved + * or copied. + * @param[in] timeout is the duration to wait for an item to appear + * in the queue (default is forever, duration::max()). + * + * @return true if a value was returned, otherwise false. + * + * @note The return value me be false if either the timeout expires + * or the queue is closed. + */ + template + bool get_until(reference val, std::chrono::time_point when) + { + lock_type lock(mutex_); + + while (queue_.empty()) + { + if (State::CLOSED == state_) + { + return false; + } + + if (empty_.wait_until(lock, when) == std::cv_status::timeout) + { + return false; + } + } + + val = std::move(queue_.front()); + queue_.pop_front(); + size_ -= 1; + + if (state_ == State::CLOSING && queue_.empty()) + { + state_ == State::CLOSED; + } + + full_.notify_one(); + + return true; + } + + /** + * Get the next item in the queue. + * + * @param[out] val is an object into which the object will be moved + * or copied. + * @param[in] timeout is the duration to wait for an item to appear + * in the queue (default is forever, duration::max()). + * + * @return true if a value was returned, otherwise false. + * + * @note The return value me be false if either the timeout expires + * or the queue is closed. + */ + template> + bool get(reference val, std::chrono::duration timeout = std::chrono::duration::max()) + { + lock_type lock(mutex_); + + while (queue_.empty()) + { + if (State::CLOSED == state_) + { + return false; + } + + if (empty_.wait_for(lock, timeout) == std::cv_status::timeout) + { + return false; + } + } + + val = std::move(queue_.front()); + queue_.pop_front(); + size_ -= 1; + + if (state_ == State::CLOSING && queue_.empty()) + { + state_ == State::CLOSED; + } + + full_.notify_one(); + + return true; + }; + + /** + * Put an item on the queue. + * + * @param[in] val is the element to be appended to the queue. + * @param[in] timeout is the duration to wait until queue there is room + * for more items on the queue (default is forever -- duration::max()). + * + * @return true if a value was put on the queue, otherwise false. + * + * @note The return value me be false if either the timeout expires + * or the queue is closed. + */ + template> + bool put(U&& val, std::chrono::duration timeout = std::chrono::duration::max()) + { + // Get the queue mutex. + lock_type lock(mutex_); + + if (SIZE == size_) + { + if (timeout.count() == 0) + { + return false; + } + + auto expiration = std::chrono::system_clock::now() + timeout; + + while (SIZE == size_) + { + if (State::OPEN != state_) + { + return false; + } + + if (full_.wait_until(lock, expiration) == std::cv_status::timeout) + { + return false; + } + } + } + + if (State::OPEN != state_) + { + return false; + } + + queue_.emplace_back(std::forward(val)); + size_ += 1; + + empty_.notify_one(); + + return true; + }; + + void close() + { + guard_type lock(mutex_); + + state_ = (queue_.empty() ? State::CLOSED : State::CLOSING); + + full_.notify_all(); + empty_.notify_all(); + } + + bool is_open() const + { + return State::OPEN == state_; + } + + bool is_closed() const + { + return State::CLOSED == state_; + } + + /** + * @return the number of items in the queue. + */ + size_t size() const + { + guard_type lock(mutex_); + return size_; + } + + /** + * @return the number of items in the queue. + */ + bool empty() const + { + guard_type lock(mutex_); + return size_ == 0; + } + + /** + * @return the capacity of the queue. + */ + static constexpr size_t capacity() + { + return SIZE; + } +}; + +} // mobilinkd diff --git a/plugins/channelrx/demodm17/m17demod.cpp b/plugins/channelrx/demodm17/m17demod.cpp index 307a36e27..0eb93f814 100644 --- a/plugins/channelrx/demodm17/m17demod.cpp +++ b/plugins/channelrx/demodm17/m17demod.cpp @@ -198,7 +198,6 @@ void M17Demod::applySettings(const M17DemodSettings& settings, bool force) << " m_inputFrequencyOffset: " << settings.m_inputFrequencyOffset << " m_rfBandwidth: " << settings.m_rfBandwidth << " m_fmDeviation: " << settings.m_fmDeviation - << " m_demodGain: " << settings.m_demodGain << " m_volume: " << settings.m_volume << " m_baudRate: " << settings.m_baudRate << " m_squelchGate" << settings.m_squelchGate @@ -218,18 +217,12 @@ void M17Demod::applySettings(const M17DemodSettings& settings, bool force) if ((settings.m_inputFrequencyOffset != m_settings.m_inputFrequencyOffset) || force) { reverseAPIKeys.append("inputFrequencyOffset"); } - if ((settings.m_demodGain != m_settings.m_demodGain) || force) { - reverseAPIKeys.append("demodGain"); - } if ((settings.m_audioMute != m_settings.m_audioMute) || force) { reverseAPIKeys.append("audioMute"); } if ((settings.m_syncOrConstellation != m_settings.m_syncOrConstellation) || force) { reverseAPIKeys.append("syncOrConstellation"); } - if ((settings.m_demodGain != m_settings.m_demodGain) || force) { - reverseAPIKeys.append("demodGain"); - } if ((settings.m_traceLengthMutliplier != m_settings.m_traceLengthMutliplier) || force) { reverseAPIKeys.append("traceLengthMutliplier"); } @@ -394,9 +387,6 @@ void M17Demod::webapiUpdateChannelSettings( if (channelSettingsKeys.contains("fmDeviation")) { settings.m_fmDeviation = response.getM17DemodSettings()->getFmDeviation(); } - if (channelSettingsKeys.contains("demodGain")) { - settings.m_demodGain = response.getM17DemodSettings()->getDemodGain(); - } if (channelSettingsKeys.contains("volume")) { settings.m_volume = response.getM17DemodSettings()->getVolume(); } @@ -478,7 +468,6 @@ void M17Demod::webapiFormatChannelSettings(SWGSDRangel::SWGChannelSettings& resp response.getM17DemodSettings()->setInputFrequencyOffset(settings.m_inputFrequencyOffset); response.getM17DemodSettings()->setRfBandwidth(settings.m_rfBandwidth); response.getM17DemodSettings()->setFmDeviation(settings.m_fmDeviation); - response.getM17DemodSettings()->setDemodGain(settings.m_demodGain); response.getM17DemodSettings()->setVolume(settings.m_volume); response.getM17DemodSettings()->setBaudRate(settings.m_baudRate); response.getM17DemodSettings()->setSquelchGate(settings.m_squelchGate); @@ -632,9 +621,6 @@ void M17Demod::webapiFormatChannelSettings( if (channelSettingsKeys.contains("fmDeviation") || force) { swgM17DemodSettings->setFmDeviation(settings.m_fmDeviation); } - if (channelSettingsKeys.contains("demodGain") || force) { - swgM17DemodSettings->setDemodGain(settings.m_demodGain); - } if (channelSettingsKeys.contains("volume") || force) { swgM17DemodSettings->setVolume(settings.m_volume); } diff --git a/plugins/channelrx/demodm17/m17demod.h b/plugins/channelrx/demodm17/m17demod.h index 788f3b7fb..7938103dc 100644 --- a/plugins/channelrx/demodm17/m17demod.h +++ b/plugins/channelrx/demodm17/m17demod.h @@ -128,6 +128,28 @@ public: void getMagSqLevels(double& avg, double& peak, int& nbSamples) { m_basebandSink->getMagSqLevels(avg, peak, nbSamples); } int getAudioSampleRate() const { return m_basebandSink->getAudioSampleRate(); } + void getDiagnostics( + bool& dcd, + float& evm, + float& deviation, + float& offset, + int& status, + float& clock, + int& sampleIndex, + int& syncIndex, + int& clockIndex, + int& viterbiCost + ) const + { + m_basebandSink->getDiagnostics(dcd, evm, deviation, offset, status, clock, sampleIndex, syncIndex, clockIndex, viterbiCost); + } + + uint32_t getLSFCount() const { return m_basebandSink->getLSFCount(); } + const QString& getSrcCall() const { return m_basebandSink->getSrcCall(); } + const QString& getDestcCall() const { return m_basebandSink->getDestcCall(); } + const QString& getTypeInfo() const { return m_basebandSink->getTypeInfo(); } + uint16_t getCRC() const { return m_basebandSink->getCRC(); } + static const char* const m_channelIdURI; static const char* const m_channelId; diff --git a/plugins/channelrx/demodm17/m17demod.ui b/plugins/channelrx/demodm17/m17demod.ui index ab952edff..4b0fd45d1 100644 --- a/plugins/channelrx/demodm17/m17demod.ui +++ b/plugins/channelrx/demodm17/m17demod.ui @@ -720,26 +720,38 @@ No Sync______ - + 10 40 - 25 - 28 + 22 + 22 + + + 0 + 0 + + - 25 - 0 + 22 + 22 + + + + + 22 + 22 Symbol synchronization rate (%) - 000 + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter diff --git a/plugins/channelrx/demodm17/m17demodbaseband.h b/plugins/channelrx/demodm17/m17demodbaseband.h index 16a85f7a2..4e8fd78f1 100644 --- a/plugins/channelrx/demodm17/m17demodbaseband.h +++ b/plugins/channelrx/demodm17/m17demodbaseband.h @@ -74,6 +74,28 @@ public: void setFifoLabel(const QString& label) { m_sampleFifo.setLabel(label); } void setAudioFifoLabel(const QString& label) { m_sink.setAudioFifoLabel(label); } + void getDiagnostics( + bool& dcd, + float& evm, + float& deviation, + float& offset, + int& status, + float& clock, + int& sampleIndex, + int& syncIndex, + int& clockIndex, + int& viterbiCost + ) const + { + m_sink.getDiagnostics(dcd, evm, deviation, offset, status, clock, sampleIndex, syncIndex, clockIndex, viterbiCost); + } + + uint32_t getLSFCount() const { return m_sink.getLSFCount(); } + const QString& getSrcCall() const { return m_sink.getSrcCall(); } + const QString& getDestcCall() const { return m_sink.getDestcCall(); } + const QString& getTypeInfo() const { return m_sink.getTypeInfo(); } + uint16_t getCRC() const { return m_sink.getCRC(); } + private: SampleSinkFifo m_sampleFifo; DownChannelizer *m_channelizer; diff --git a/plugins/channelrx/demodm17/m17demodfilters.cpp b/plugins/channelrx/demodm17/m17demodfilters.cpp new file mode 100644 index 000000000..70e9ee49a --- /dev/null +++ b/plugins/channelrx/demodm17/m17demodfilters.cpp @@ -0,0 +1,48 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022 Edouard Griffiths, F4EXB. // +// // +// 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 // +// // +// 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 . // +/////////////////////////////////////////////////////////////////////////////////// + +#include "m17demodfilters.h" + +const float M17DemodAudioInterpolatorFilter::m_lpa[3] = {1.0, 1.392667E+00, -5.474446E-01}; +const float M17DemodAudioInterpolatorFilter::m_lpb[3] = {3.869430E-02, 7.738860E-02, 3.869430E-02}; +// f(-3dB) = 300 Hz @ 8000 Hz SR (w = 0.075): +const float M17DemodAudioInterpolatorFilter::m_hpa[3] = {1.000000e+00, 1.667871e+00, -7.156964e-01}; +const float M17DemodAudioInterpolatorFilter::m_hpb[3] = {8.459039e-01, -1.691760e+00, 8.459039e-01}; + +M17DemodAudioInterpolatorFilter::M17DemodAudioInterpolatorFilter() : + m_filterLP(m_lpa, m_lpb), + m_filterHP(m_hpa, m_hpb), + m_useHP(false) +{ +} + +M17DemodAudioInterpolatorFilter::~M17DemodAudioInterpolatorFilter() +{} + +float M17DemodAudioInterpolatorFilter::run(const float& sample) +{ + return m_useHP ? m_filterLP.run(m_filterHP.run(sample)) : m_filterLP.run(sample); +} + +float M17DemodAudioInterpolatorFilter::runHP(const float& sample) +{ + return m_filterHP.run(sample); +} + +float M17DemodAudioInterpolatorFilter::runLP(const float& sample) +{ + return m_filterLP.run(sample); +} diff --git a/plugins/channelrx/demodm17/m17demodfilters.h b/plugins/channelrx/demodm17/m17demodfilters.h new file mode 100644 index 000000000..b95c40300 --- /dev/null +++ b/plugins/channelrx/demodm17/m17demodfilters.h @@ -0,0 +1,66 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022 Edouard Griffiths, F4EXB. // +// // +// 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 // +// // +// 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 . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef INCLUDE_M17DEMODFILTERS_H +#define INCLUDE_M17DEMODFILTERS_H + +#define NZEROS 60 +#define NXZEROS 134 + +#include "dsp/iirfilter.h" +#include "export.h" + +/** + * This is a 2 pole lowpass Chebyshev (recursive) filter at fc=0.075 using coefficients found in table 20-1 of + * http://www.analog.com/media/en/technical-documentation/dsp-book/dsp_book_Ch20.pdf + * + * At the interpolated sampling frequency of 48 kHz the -3 dB corner is at 48 * .075 = 3.6 kHz which is perfect for voice + * + * a0= 3.869430E-02 + * a1= 7.738860E-02 b1= 1.392667E+00 + * a2= 3.869430E-02 b2= -5.474446E-01 + * + * given x[n] is the new input sample and y[n] the returned output sample: + * + * y[n] = a0*x[n] + a1*x[n] + a2*x[n] + b1*y[n-1] + b2*y[n-2] + * + * This one works directly with floats + * + */ + +class M17DemodAudioInterpolatorFilter +{ +public: + M17DemodAudioInterpolatorFilter(); + ~M17DemodAudioInterpolatorFilter(); + + void useHP(bool useHP) { m_useHP = useHP; } + bool usesHP() const { return m_useHP; } + float run(const float& sample); + float runHP(const float& sample); + float runLP(const float& sample); + +private: + IIRFilter m_filterLP; + IIRFilter m_filterHP; + bool m_useHP; + static const float m_lpa[3]; + static const float m_lpb[3]; + static const float m_hpa[3]; + static const float m_hpb[3]; +}; + +#endif diff --git a/plugins/channelrx/demodm17/m17demodgui.cpp b/plugins/channelrx/demodm17/m17demodgui.cpp index 01c45873f..b5c205aed 100644 --- a/plugins/channelrx/demodm17/m17demodgui.cpp +++ b/plugins/channelrx/demodm17/m17demodgui.cpp @@ -148,13 +148,6 @@ void M17DemodGUI::on_rfBW_valueChanged(int value) applySettings(); } -void M17DemodGUI::on_demodGain_valueChanged(int value) -{ - m_settings.m_demodGain = value / 100.0; - ui->demodGainText->setText(QString("%1").arg(value / 100.0, 0, 'f', 2)); - applySettings(); -} - void M17DemodGUI::on_fmDeviation_valueChanged(int value) { m_settings.m_fmDeviation = value * 100.0; @@ -301,11 +294,9 @@ M17DemodGUI::M17DemodGUI(PluginAPI* pluginAPI, DeviceUISet *deviceUISet, Baseban m_doApplySettings(true), m_enableCosineFiltering(false), m_syncOrConstellation(false), - m_slot1On(false), - m_slot2On(false), - m_tdmaStereo(false), m_squelchOpen(false), m_audioSampleRate(-1), + m_lsfCount(0), m_tickCount(0) { setAttribute(Qt::WA_DeleteOnClose, true); @@ -370,6 +361,9 @@ M17DemodGUI::M17DemodGUI(PluginAPI* pluginAPI, DeviceUISet *deviceUISet, Baseban m_settings.setChannelMarker(&m_channelMarker); m_settings.setRollupState(&m_rollupState); + ui->dcdLabel->setPixmap(QIcon(":/carrier.png").pixmap(QSize(20, 20))); + ui->lockLabel->setPixmap(QIcon(":/locked.png").pixmap(QSize(20, 20))); + updateMyPosition(); displaySettings(); makeUIConnections(); @@ -425,9 +419,6 @@ void M17DemodGUI::displaySettings() ui->squelchGate->setValue(m_settings.m_squelchGate); ui->squelchGateText->setText(QString("%1").arg(ui->squelchGate->value() * 10.0, 0, 'f', 0)); - ui->demodGain->setValue(m_settings.m_demodGain * 100.0); - ui->demodGainText->setText(QString("%1").arg(ui->demodGain->value() / 100.0, 0, 'f', 2)); - ui->volume->setValue(m_settings.m_volume * 10.0); ui->volumeText->setText(QString("%1").arg(ui->volume->value() / 10.0, 0, 'f', 1)); @@ -543,14 +534,77 @@ void M17DemodGUI::tick() m_squelchOpen = squelchOpen; } + if (m_tickCount % 10 == 0) + { + bool dcd; + float evm; + float deviation; + float offset; + int status; + float clock; + int sampleIndex; + int syncIndex; + int clockIndex; + int viterbiCost; + + m_m17Demod->getDiagnostics(dcd, evm, deviation, offset, status, clock, sampleIndex, syncIndex, clockIndex, viterbiCost); + + if (dcd) { + ui->dcdLabel->setStyleSheet("QLabel { background-color : green; }"); + } else { + ui->dcdLabel->setStyleSheet(tr("QLabel { background-color : %1; }").arg(palette().button().color().name())); + } + + if (status == 0) { // unlocked + ui->lockLabel->setStyleSheet(tr("QLabel { background-color : %1; }").arg(palette().button().color().name())); + } else { + ui->lockLabel->setStyleSheet("QLabel { background-color : green; }"); + } + + ui->syncText->setText(getStatus(status)); + ui->evmText->setText(tr("%1").arg(evm*100.0f, 3, 'f', 1)); + ui->deviationText->setText(tr("%1").arg(deviation, 2, 'f', 1)); + ui->offsetText->setText(tr("%1").arg(offset, 3, 'f', 2)); + ui->viterbiText->setText(tr("%1").arg(viterbiCost)); + ui->clockText->setText(tr("%1").arg(clock, 2, 'f', 1)); + ui->sampleText->setText(tr("%1, %2, %3").arg(sampleIndex).arg(syncIndex).arg(clockIndex)); + + if (m_m17Demod->getLSFCount() != m_lsfCount) + { + ui->sourceText->setText(m_m17Demod->getSrcCall()); + ui->destText->setText(m_m17Demod->getDestcCall()); + ui->typeText->setText(m_m17Demod->getTypeInfo()); + ui->crcText->setText(tr("%1").arg(m_m17Demod->getCRC(), 4, 16, QChar('0'))); + m_lsfCount = m_m17Demod->getLSFCount(); + } + } + m_tickCount++; } +QString M17DemodGUI::getStatus(int status) +{ + if (status == 0) { + return "Unlocked"; + } else if (status == 1) { + return "LSF"; + } else if (status == 2) { + return "Stream"; + } else if (status == 3) { + return "Packet"; + } else if (status == 4) { + return "BERT"; + } else if (status == 5) { + return "Frame"; + } else { + return "Unknown"; + } +} + void M17DemodGUI::makeUIConnections() { QObject::connect(ui->deltaFrequency, &ValueDialZ::changed, this, &M17DemodGUI::on_deltaFrequency_changed); QObject::connect(ui->rfBW, &QSlider::valueChanged, this, &M17DemodGUI::on_rfBW_valueChanged); - QObject::connect(ui->demodGain, &QSlider::valueChanged, this, &M17DemodGUI::on_demodGain_valueChanged); QObject::connect(ui->volume, &QDial::valueChanged, this, &M17DemodGUI::on_volume_valueChanged); QObject::connect(ui->baudRate, QOverload::of(&QComboBox::currentIndexChanged), this, &M17DemodGUI::on_baudRate_currentIndexChanged); QObject::connect(ui->syncOrConstellation, &QToolButton::toggled, this, &M17DemodGUI::on_syncOrConstellation_toggled); diff --git a/plugins/channelrx/demodm17/m17demodgui.h b/plugins/channelrx/demodm17/m17demodgui.h index 24973446e..301e36dbd 100644 --- a/plugins/channelrx/demodm17/m17demodgui.h +++ b/plugins/channelrx/demodm17/m17demodgui.h @@ -87,12 +87,10 @@ private: M17Demod* m_m17Demod; bool m_enableCosineFiltering; bool m_syncOrConstellation; - bool m_slot1On; - bool m_slot2On; - bool m_tdmaStereo; bool m_audioMute; bool m_squelchOpen; int m_audioSampleRate; + uint32_t m_lsfCount; uint32_t m_tickCount; float m_myLatitude; @@ -112,6 +110,7 @@ private: bool handleMessage(const Message& message); void makeUIConnections(); void updateAbsoluteCenterFrequency(); + QString getStatus(int status); void leaveEvent(QEvent*); void enterEvent(QEvent*); @@ -119,7 +118,6 @@ private: private slots: void on_deltaFrequency_changed(qint64 value); void on_rfBW_valueChanged(int index); - void on_demodGain_valueChanged(int value); void on_volume_valueChanged(int value); void on_baudRate_currentIndexChanged(int index); void on_syncOrConstellation_toggled(bool checked); diff --git a/plugins/channelrx/demodm17/m17demodgui.ui b/plugins/channelrx/demodm17/m17demodgui.ui index 8749f11c4..a76e3db5f 100644 --- a/plugins/channelrx/demodm17/m17demodgui.ui +++ b/plugins/channelrx/demodm17/m17demodgui.ui @@ -6,7 +6,7 @@ 0 0 - 530 + 482 392 @@ -18,7 +18,7 @@ - 530 + 482 392 @@ -42,7 +42,7 @@ 0 0 - 528 + 480 172 @@ -54,7 +54,7 @@ - 528 + 480 0 @@ -532,7 +532,7 @@ - Source + Src @@ -544,6 +544,15 @@ 0 + + Source callsign + + + QFrame::Box + + + QFrame::Sunken + ... @@ -552,7 +561,7 @@ - Dest + Dst @@ -564,6 +573,73 @@ 0 + + Destination callsign + + + QFrame::Box + + + QFrame::Sunken + + + ... + + + + + + + Typ + + + + + + + + 110 + 0 + + + + Data stream type information + + + QFrame::Box + + + QFrame::Sunken + + + ... + + + + + + + CRC + + + + + + + + 30 + 0 + + + + CRC for the LSF data + + + QFrame::Box + + + QFrame::Sunken + ... @@ -589,7 +665,7 @@ - 10 + 0 180 480 210 @@ -646,8 +722,8 @@ 10 10 - 59 - 20 + 60 + 24 @@ -683,7 +759,7 @@ 80 10 110 - 25 + 24 @@ -695,7 +771,7 @@ 16777215 - 25 + 24 @@ -705,7 +781,7 @@ - Synchronized on this frame type + Synchronization status QFrame::Box @@ -714,66 +790,59 @@ QFrame::Sunken - 2 + 1 No Sync______ - + - 10 - 40 - 25 - 28 + 194 + 10 + 24 + 24 + + + 0 + 0 + + - 25 - 0 + 24 + 24 + + + + + 24 + 24 - Symbol synchronization rate (%) + Data Carrier Detect (DCD) + + + QFrame::Box + + + QFrame::Sunken - 000 + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + - 40 - 40 - 25 - 28 - - - - - 25 - 0 - - - - Zero crossing relative position in number of samples (<0 sampling point lags, >0 it leads) - - - -00 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - 80 + 84 40 25 28 @@ -789,13 +858,13 @@ Carrier relative position (%) when synchronized - -00 + Dev - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + 110 @@ -814,7 +883,7 @@ Carrier input level (%) when synchronized - 000 + 0.0 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -824,7 +893,7 @@ 10 - 70 + 100 23 22 @@ -848,9 +917,9 @@ 50 - 107 - 141 - 16 + 135 + 154 + 22 @@ -876,7 +945,7 @@ 10 - 100 + 130 25 29 @@ -888,8 +957,8 @@ - 200 - 100 + 205 + 130 50 29 @@ -907,58 +976,11 @@ Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - 10 - 130 - 28 - 30 - - - - Gain - - - - - - 50 - 137 - 141 - 16 - - - - - 0 - 0 - - - - Gain after discriminator - - - 50 - - - 200 - - - 1 - - - 100 - - - Qt::Horizontal - - 40 - 68 + 100 24 24 @@ -988,10 +1010,10 @@ - 70 - 73 + 68 + 100 31 - 16 + 22 @@ -1004,33 +1026,11 @@ Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - 200 - 130 - 50 - 29 - - - - - 50 - 0 - - - - 0.00 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 110 - 70 + 108 + 100 24 24 @@ -1060,10 +1060,10 @@ - 130 - 73 + 128 + 100 31 - 16 + 22 @@ -1079,8 +1079,8 @@ - 170 - 70 + 168 + 100 24 24 @@ -1110,10 +1110,10 @@ - 190 - 73 + 188 + 100 31 - 16 + 22 @@ -1126,6 +1126,299 @@ Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + 222 + 10 + 24 + 24 + + + + + 0 + 0 + + + + + 24 + 24 + + + + + 24 + 24 + + + + Locked state + + + QFrame::Box + + + QFrame::Sunken + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 10 + 40 + 27 + 28 + + + + + 27 + 0 + + + + Carrier relative position (%) when synchronized + + + EVM + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 40 + 40 + 38 + 28 + + + + + 30 + 0 + + + + Error Vector Magnitude (%) when synchronized + + + 00.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 142 + 40 + 25 + 28 + + + + + 25 + 0 + + + + Carrier relative position (%) when synchronized + + + Ofs + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + 170 + 40 + 35 + 28 + + + + + 25 + 0 + + + + Carrier input level (%) when synchronized + + + 0.00 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 178 + 65 + 25 + 28 + + + + + 25 + 0 + + + + Carrier relative position (%) when synchronized + + + Vit + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + 203 + 65 + 25 + 28 + + + + + 25 + 0 + + + + Carrier input level (%) when synchronized + + + 128 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 10 + 65 + 25 + 28 + + + + + 25 + 0 + + + + Carrier relative position (%) when synchronized + + + Clk + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + 40 + 65 + 25 + 28 + + + + + 25 + 0 + + + + Carrier input level (%) when synchronized + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 80 + 65 + 35 + 28 + + + + + 25 + 0 + + + + Carrier relative position (%) when synchronized + + + Samp + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + 120 + 65 + 46 + 28 + + + + + 25 + 0 + + + + Carrier input level (%) when synchronized + + + 0, 0, 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + diff --git a/plugins/channelrx/demodm17/m17demodprocessor.cpp b/plugins/channelrx/demodm17/m17demodprocessor.cpp new file mode 100644 index 000000000..8cbaf4fe1 --- /dev/null +++ b/plugins/channelrx/demodm17/m17demodprocessor.cpp @@ -0,0 +1,494 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022 Edouard Griffiths, F4EXB // +// // +// 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 . // +/////////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include + +#include + +#include "audio/audiofifo.h" + +#include "m17/ax25_frame.h" +#include "m17demodprocessor.h" + +M17DemodProcessor* M17DemodProcessor::m_this = nullptr; + +M17DemodProcessor::M17DemodProcessor() : + m_packetFrameCounter(0), + m_displayLSF(true), + m_noiseBlanker(true), + m_demod(handle_frame), + m_audioFifo(nullptr), + m_audioMute(false), + m_volume(1.0f) +{ + m_this = this; + m_codec2 = ::codec2_create(CODEC2_MODE_3200); + m_audioBuffer.resize(48000); + m_audioBufferFill = 0; + m_srcCall = ""; + m_destCall = ""; + m_typeInfo = ""; + m_metadata.fill(0); + m_crc = 0; + m_lsfCount = 0; + setUpsampling(6); // force upsampling of audio to 48k + m_demod.diagnostics(diagnostic_callback); +} + +M17DemodProcessor::~M17DemodProcessor() +{ + codec2_destroy(m_codec2); +} + +void M17DemodProcessor::pushSample(qint16 sample) +{ + m_demod(sample / 22000.0f); +} + +bool M17DemodProcessor::handle_frame(mobilinkd::M17FrameDecoder::output_buffer_t const& frame, int viterbi_cost) +{ + using FrameType = mobilinkd::M17FrameDecoder::FrameType; + + bool result = true; + + switch (frame.type) + { + case FrameType::LSF: + result = m_this->decode_lsf(frame.lsf); + break; + case FrameType::LICH: + result = m_this->decode_lich(frame.lich); + break; + case FrameType::STREAM: + result = m_this->demodulate_audio(frame.stream, viterbi_cost); + break; + case FrameType::BASIC_PACKET: + result = m_this->decode_packet(frame.packet); + break; + case FrameType::FULL_PACKET: + result = m_this->decode_packet(frame.packet); + break; + case FrameType::BERT: + result = m_this->decode_bert(frame.bert); + break; + } + + return result; +} + +void M17DemodProcessor::diagnostic_callback( + bool dcd, + float evm, + float deviation, + float offset, + int status, + float clock, + int sample_index, + int sync_index, + int clock_index, + int viterbi_cost) +{ + bool debug = false; + bool quiet = true; + + m_this->m_dcd = dcd; + m_this->m_evm = evm; + m_this->m_deviation = deviation; + m_this->m_offset = offset; + m_this->m_status = status; + m_this->m_clock = clock; + m_this->m_sampleIndex = sample_index; + m_this->m_syncIndex = sync_index; + m_this->m_clockIndex = clock_index; + m_this->m_viterbiCost = viterbi_cost; + + if (debug) + { + std::ostringstream oss; + oss << "dcd: " << std::setw(1) << int(dcd) + << ", evm: " << std::setfill(' ') << std::setprecision(4) << std::setw(8) << evm * 100 <<"%" + << ", deviation: " << std::setprecision(4) << std::setw(8) << deviation + << ", freq offset: " << std::setprecision(4) << std::setw(8) << offset + << ", locked: " << std::boolalpha << std::setw(6) << (status != 0) << std::dec + << ", clock: " << std::setprecision(7) << std::setw(8) << clock + << ", sample: " << std::setw(1) << sample_index << ", " << sync_index << ", " << clock_index + << ", cost: " << viterbi_cost; + qDebug() << "M17DemodProcessor::diagnostic_callback: " << oss.str().c_str(); + } + + if (!dcd && m_this->m_prbs.sync()) { // Seems like there should be a better way to do this. + m_this->m_prbs.reset(); + } + + if (m_this->m_prbs.sync() && !quiet) + { + std::ostringstream oss; + auto ber = double(m_this->m_prbs.errors()) / double(m_this->m_prbs.bits()); + char buffer[40]; + snprintf(buffer, 40, "BER: %-1.6lf (%u bits)", ber, m_this->m_prbs.bits()); + oss << buffer; + qDebug() << "M17DemodProcessor::diagnostic_callback: " << oss.str().c_str(); + } + + if (status == 0) { // unlocked + m_this->resetInfo(); + } +} + +bool M17DemodProcessor::decode_lich(mobilinkd::M17FrameDecoder::lich_buffer_t const& lich) +{ + uint8_t fragment_number = lich[5]; // Get fragment number. + fragment_number = (fragment_number >> 5) & 7; + qDebug("M17DemodProcessor::handle_frame: LICH: %d", (int) fragment_number); + return true; +} + +bool M17DemodProcessor::decode_lsf(mobilinkd::M17FrameDecoder::lsf_buffer_t const& lsf) +{ + mobilinkd::LinkSetupFrame::encoded_call_t encoded_call; + std::ostringstream oss; + + std::copy(lsf.begin() + 6, lsf.begin() + 12, encoded_call.begin()); + mobilinkd::LinkSetupFrame::call_t src = mobilinkd::LinkSetupFrame::decode_callsign(encoded_call); + m_srcCall = QString(src.data()); + + std::copy(lsf.begin(), lsf.begin() + 6, encoded_call.begin()); + mobilinkd::LinkSetupFrame::call_t dest = mobilinkd::LinkSetupFrame::decode_callsign(encoded_call); + m_destCall = QString(dest.data()); + + uint16_t type = (lsf[12] << 8) | lsf[13]; + decode_type(type); + + std::copy(lsf.begin()+14, lsf.begin()+28, m_metadata.begin()); + m_crc = (lsf[28] << 8) | lsf[29]; + + if (m_displayLSF) + { + oss << "SRC: " << m_srcCall.toStdString().c_str(); + oss << ", DEST: " << m_destCall.toStdString().c_str(); + oss << ", " << m_typeInfo.toStdString().c_str(); + oss << ", META: "; + for (size_t i = 0; i != 14; ++i) { + oss << std::hex << std::setw(2) << std::setfill('0') << int(m_metadata[i]); + } + oss << ", CRC: " << std::hex << std::setw(4) << std::setfill('0') << m_crc; + oss << std::dec; + } + + m_currentPacket.clear(); + m_packetFrameCounter = 0; + + if (!lsf[111]) // LSF type bit 0 + { + uint8_t packet_type = (lsf[109] << 1) | lsf[110]; + + switch (packet_type) + { + case 1: // RAW -- ignore LSF. + break; + case 2: // ENCAPSULATED + append_packet(m_currentPacket, lsf); + break; + default: + oss << " LSF for reserved packet type"; + append_packet(m_currentPacket, lsf); + } + } + + qDebug() << "M17DemodProcessor::decode_lsf: " << oss.str().c_str(); + m_lsfCount++; + return true; +} + +void M17DemodProcessor::decode_type(uint16_t type) +{ + if (type & 1) // bit 0 + { + m_typeInfo = "STR:"; // Stream mode + + switch ((type & 6) >> 1) // bits 1..2 + { + case 0: + m_typeInfo += "UNK"; + break; + case 1: + m_typeInfo += "D/D"; + break; + case 2: + m_typeInfo += "V/V"; + break; + case 3: + m_typeInfo += "V/D"; + break; + } + } + else + { + m_typeInfo = "PKT:"; // Packet mode + + switch ((type & 6) >> 1) // bits 1..2 + { + case 0: + m_typeInfo += "UNK"; + break; + case 1: + m_typeInfo += "RAW"; + break; + case 2: + m_typeInfo += "ENC"; + break; + case 3: + m_typeInfo += "UNK"; + break; + } + } + + m_typeInfo += QString(" CAN:%1").arg(int((type & 0x780) >> 7), 2, 10, QChar('0')); // Channel Access number (bits 7..10) +} + +void M17DemodProcessor::resetInfo() +{ + m_srcCall = ""; + m_destCall = ""; + m_typeInfo = ""; + m_metadata.fill(0); + m_crc = 0; + m_lsfCount = 0; +} + +void M17DemodProcessor::setDCDOff() +{ + qDebug("M17DemodProcessor::setDCDOff"); + m_demod.dcd_off(); +} + +void M17DemodProcessor::append_packet(std::vector& result, mobilinkd::M17FrameDecoder::lsf_buffer_t in) +{ + uint8_t out = 0; + size_t b = 0; + + for (auto c : in) + { + out = (out << 1) | c; + if (++b == 8) + { + result.push_back(out); + out = 0; + b = 0; + } + } +} + +bool M17DemodProcessor::decode_packet(mobilinkd::M17FrameDecoder::packet_buffer_t const& packet_segment) +{ + if (packet_segment[25] & 0x80) // last frame of packet. + { + size_t packet_size = (packet_segment[25] & 0x7F) >> 2; + packet_size = std::min(packet_size, size_t(25)); + + for (size_t i = 0; i != packet_size; ++i) { + m_currentPacket.push_back(packet_segment[i]); + } + + boost::crc_optimal<16, 0x1021, 0xFFFF, 0xFFFF, true, true> crc; + crc.process_bytes(&m_currentPacket.front(), m_currentPacket.size()); + uint16_t checksum = crc.checksum(); + + if (checksum == 0x0f47) + { + std::string ax25; + ax25.reserve(m_currentPacket.size()); + + for (auto c : m_currentPacket) { + ax25.push_back(char(c)); + } + + mobilinkd::ax25_frame frame(ax25); + std::ostringstream oss; + mobilinkd::write(oss, frame); // TODO: get details + qDebug() << "M17DemodProcessor::decode_packet: " << oss.str().c_str(); + return true; + } + + qWarning() << "M17DemodProcessor::decode_packet: Packet checksum error: " << std::hex << checksum << std::dec; + return false; + } + + size_t frame_number = (packet_segment[25] & 0x7F) >> 2; + + if (frame_number != m_packetFrameCounter) + { + qWarning() << "M17DemodProcessor::decode_packet: Packet frame sequence error. Got " + << frame_number << ", expected " << m_packetFrameCounter; + return false; + } + + m_packetFrameCounter++; + + for (size_t i = 0; i != 25; ++i) { + m_currentPacket.push_back(packet_segment[i]); + } + + return true; +} + +bool M17DemodProcessor::decode_bert(mobilinkd::M17FrameDecoder::bert_buffer_t const& bert) +{ + for (int j = 0; j != 24; ++j) + { + auto b = bert[j]; + + for (int i = 0; i != 8; ++i) + { + m_prbs.validate(b & 0x80); + b <<= 1; + } + } + + auto b = bert[24]; + + for (int i = 0; i != 5; ++i) + { + m_prbs.validate(b & 0x80); + b <<= 1; + } + + return true; +} + +bool M17DemodProcessor::demodulate_audio(mobilinkd::M17FrameDecoder::audio_buffer_t const& audio, int viterbi_cost) +{ + bool result = true; + std::array buf; // 8k audio + + // First two bytes are the frame counter + EOS indicator. + if (viterbi_cost < 70 && (audio[0] & 0x80)) + { + if (m_displayLSF) { + qDebug() << "M17DemodProcessor::demodulate_audio: EOS"; + } + + result = false; + } + + if (m_audioFifo && !m_audioMute) + { + if (m_noiseBlanker && viterbi_cost > 80) + { + buf.fill(0); + processAudio(buf); // first block expanded + processAudio(buf); // second block expanded + } + else + { + codec2_decode(m_codec2, buf.data(), audio.data() + 2); // first 8 bytes block input + processAudio(buf); + codec2_decode(m_codec2, buf.data(), audio.data() + 10); // second 8 bytes block input + processAudio(buf); + } + } + + return result; +} + +void M17DemodProcessor::setUpsampling(int upsampling) +{ + m_upsampling = upsampling < 1 ? 1 : upsampling > 6 ? 6 : upsampling; +} + +void M17DemodProcessor::setVolume(float volume) +{ + m_volume = volume; + setVolumeFactors(); +} + +void M17DemodProcessor::processAudio(const std::array& in) +{ + if (m_upsampling > 1) { + upsample(m_upsampling, in.data(), in.size()); + } else { + noUpsample(in.data(), in.size()); + } + + if (m_audioBufferFill >= m_audioBuffer.size() - 960) + { + uint res = m_audioFifo->write((const quint8*)&m_audioBuffer[0], m_audioBufferFill); + + if (res != m_audioBufferFill) { + qDebug("M17DemodProcessor::processAudio: %u/%u audio samples written", res, m_audioBufferFill); + } + + m_audioBufferFill = 0; + } +} + +void M17DemodProcessor::upsample(int upsampling, const int16_t *in, int nbSamplesIn) +{ + for (int i = 0; i < nbSamplesIn; i++) + { + float cur = m_upsamplingFilter.usesHP() ? m_upsamplingFilter.runHP((float) in[i]) : (float) in[i]; + float prev = m_upsamplerLastValue; + qint16 upsample; + + for (int j = 1; j <= upsampling; j++) + { + upsample = (qint16) m_upsamplingFilter.runLP(cur*m_upsamplingFactors[j] + prev*m_upsamplingFactors[upsampling-j]); + m_audioBuffer[m_audioBufferFill].l = upsample; //m_compressor.compress(upsample); + m_audioBuffer[m_audioBufferFill].r = upsample; //m_compressor.compress(upsample); + + if (m_audioBufferFill < m_audioBuffer.size() - 1) { + ++m_audioBufferFill; + } + } + + m_upsamplerLastValue = cur; + } + + if (m_audioBufferFill >= m_audioBuffer.size() - 1) { + qDebug("M17DemodProcessor::upsample(%d): audio buffer is full check its size", upsampling); + } +} + +void M17DemodProcessor::noUpsample(const int16_t *in, int nbSamplesIn) +{ + for (int i = 0; i < nbSamplesIn; i++) + { + float cur = m_upsamplingFilter.usesHP() ? m_upsamplingFilter.runHP((float) in[i]) : (float) in[i]; + m_audioBuffer[m_audioBufferFill].l = cur*m_upsamplingFactors[0]; + m_audioBuffer[m_audioBufferFill].r = cur*m_upsamplingFactors[0]; + + if (m_audioBufferFill < m_audioBuffer.size() - 1) { + ++m_audioBufferFill; + } + } + + if (m_audioBufferFill >= m_audioBuffer.size() - 1) { + qDebug("M17DemodProcessor::noUpsample: audio buffer is full check its size"); + } +} + +void M17DemodProcessor::setVolumeFactors() +{ + m_upsamplingFactors[0] = m_volume; + + for (int i = 1; i <= m_upsampling; i++) { + m_upsamplingFactors[i] = (i*m_volume) / (float) m_upsampling; + } +} diff --git a/plugins/channelrx/demodm17/m17demodprocessor.h b/plugins/channelrx/demodm17/m17demodprocessor.h new file mode 100644 index 000000000..fa9c96105 --- /dev/null +++ b/plugins/channelrx/demodm17/m17demodprocessor.h @@ -0,0 +1,145 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022 Edouard Griffiths, F4EXB // +// // +// 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 . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef INCLUDE_M17DEMODPROCESSOR_H +#define INCLUDE_M17DEMODPROCESSOR_H + +#include + +#include "audio/audiocompressor.h" +#include "m17/M17Demodulator.h" +#include "m17demodfilters.h" + +class AudioFifo; + +class M17DemodProcessor : public QObject +{ + Q_OBJECT +public: + M17DemodProcessor(); + ~M17DemodProcessor(); + + void pushSample(qint16 sample); + void setDisplayLSF(bool displayLSF) { m_displayLSF = displayLSF; } + void setNoiseBlanker(bool noiseBlanker) { m_noiseBlanker = noiseBlanker; } + void setAudioFifo(AudioFifo *fifo) { m_audioFifo = fifo; } + void setAudioMute(bool mute) { m_audioMute = mute; } + void setUpsampling(int upsampling); + void setVolume(float volume); + void setHP(bool useHP) { m_upsamplingFilter.useHP(useHP); } + void resetInfo(); + void setDCDOff(); + uint32_t getLSFCount() const { return m_lsfCount; } + const QString& getSrcCall() const { return m_srcCall; } + const QString& getDestcCall() const { return m_destCall; } + const QString& getTypeInfo() const { return m_typeInfo; } + uint16_t getCRC() const { return m_crc; } + + void getDiagnostics( + bool& dcd, + float& evm, + float& deviation, + float& offset, + int& status, + float& clock, + int& sampleIndex, + int& syncIndex, + int& clockIndex, + int& viterbiCost + ) const + { + dcd = m_dcd; + evm = m_evm; + deviation = m_deviation; + offset = m_offset; + status = m_status; + clock = m_clock; + sampleIndex = m_sampleIndex; + syncIndex = m_syncIndex; + clockIndex = m_clockIndex; + viterbiCost = m_viterbiCost; + } + +private: + std::vector m_currentPacket; + size_t m_packetFrameCounter; + mobilinkd::PRBS9 m_prbs; + bool m_displayLSF; + bool m_noiseBlanker; + struct CODEC2 *m_codec2; + static M17DemodProcessor *m_this; + mobilinkd::M17Demodulator m_demod; + AudioFifo *m_audioFifo; + bool m_audioMute; + AudioVector m_audioBuffer; + uint m_audioBufferFill; + float m_volume; + int m_upsampling; //!< upsampling factor + float m_upsamplingFactors[7]; + AudioCompressor m_compressor; + float m_upsamplerLastValue; + + M17DemodAudioInterpolatorFilter m_upsamplingFilter; + + // Diagnostics + bool m_dcd; //!< Data Carrier Detect + float m_evm; //!< Error Vector Magnitude in percent + float m_deviation; //!< Estimated deviation. Ideal = 1.0 + float m_offset; //!< Estimated frequency offset. Ideal = 0.0 practically limited to ~[-0.18, 0.18] + int m_status; //!< Status + float m_clock; + int m_sampleIndex; + int m_syncIndex; + int m_clockIndex; + int m_viterbiCost; //!< [-1:128] ideally 0 + + QString m_srcCall; + QString m_destCall; + QString m_typeInfo; + std::array m_metadata; + uint16_t m_crc; + uint32_t m_lsfCount; // Incremented each time a new LSF is decoded. Reset when lock is lost. + + static bool handle_frame(mobilinkd::M17FrameDecoder::output_buffer_t const& frame, int viterbi_cost); + static void diagnostic_callback( + bool dcd, + float evm, + float deviation, + float offset, + int status, + float clock, + int sample_index, + int sync_index, + int clock_index, + int viterbi_cost + ); + bool decode_lsf(mobilinkd::M17FrameDecoder::lsf_buffer_t const& lsf); + bool decode_lich(mobilinkd::M17FrameDecoder::lich_buffer_t const& lich); + bool decode_packet(mobilinkd::M17FrameDecoder::packet_buffer_t const& packet_segment); + bool decode_bert(mobilinkd::M17FrameDecoder::bert_buffer_t const& bert); + bool demodulate_audio(mobilinkd::M17FrameDecoder::audio_buffer_t const& audio, int viterbi_cost); + void decode_type(uint16_t type); + void append_packet(std::vector& result, mobilinkd::M17FrameDecoder::lsf_buffer_t in); + + void processAudio(const std::array& in); + void upsample(int upsampling, const int16_t *in, int nbSamplesIn); + void noUpsample(const int16_t *in, int nbSamplesIn); + + void setVolumeFactors(); +}; + +#endif // INCLUDE_M17PROCESSOR_H diff --git a/plugins/channelrx/demodm17/m17demodsettings.cpp b/plugins/channelrx/demodm17/m17demodsettings.cpp index 2fc653a27..64dcb0bc1 100644 --- a/plugins/channelrx/demodm17/m17demodsettings.cpp +++ b/plugins/channelrx/demodm17/m17demodsettings.cpp @@ -35,7 +35,6 @@ void M17DemodSettings::resetToDefaults() m_inputFrequencyOffset = 0; m_rfBandwidth = 12500.0; m_fmDeviation = 3500.0; - m_demodGain = 1.0; m_volume = 2.0; m_baudRate = 4800; m_squelchGate = 5; // 10s of ms at 48000 Hz sample rate. Corresponds to 2400 for AGC attack @@ -64,7 +63,6 @@ QByteArray M17DemodSettings::serialize() const SimpleSerializer s(1); s.writeS32(1, m_inputFrequencyOffset); s.writeS32(2, m_rfBandwidth/100.0); - s.writeS32(3, m_demodGain*100.0); s.writeS32(4, m_fmDeviation/100.0); s.writeS32(5, m_squelch); s.writeU32(7, m_rgbColor); @@ -130,7 +128,6 @@ bool M17DemodSettings::deserialize(const QByteArray& data) d.readS32(2, &tmp, 125); m_rfBandwidth = tmp * 100.0; d.readS32(3, &tmp, 125); - m_demodGain = tmp / 100.0; d.readS32(4, &tmp, 50); m_fmDeviation = tmp * 100.0; d.readS32(5, &tmp, -40); diff --git a/plugins/channelrx/demodm17/m17demodsettings.h b/plugins/channelrx/demodm17/m17demodsettings.h index 1ebd6e5b6..79710297c 100644 --- a/plugins/channelrx/demodm17/m17demodsettings.h +++ b/plugins/channelrx/demodm17/m17demodsettings.h @@ -30,7 +30,6 @@ struct M17DemodSettings qint64 m_inputFrequencyOffset; Real m_rfBandwidth; Real m_fmDeviation; - Real m_demodGain; Real m_volume; int m_baudRate; int m_squelchGate; diff --git a/plugins/channelrx/demodm17/m17demodsink.cpp b/plugins/channelrx/demodm17/m17demodsink.cpp index f762f0781..dea62c74e 100644 --- a/plugins/channelrx/demodm17/m17demodsink.cpp +++ b/plugins/channelrx/demodm17/m17demodsink.cpp @@ -53,6 +53,7 @@ M17DemodSink::M17DemodSink() : m_squelchGate(0), m_squelchLevel(1e-4), m_squelchOpen(false), + m_squelchWasOpen(false), m_squelchDelayLine(24000), m_audioFifo(48000), m_scopeXY(nullptr), @@ -62,6 +63,7 @@ M17DemodSink::M17DemodSink() : m_audioBufferFill = 0; m_demodBuffer.resize(1<<12); m_demodBufferFill = 0; + m_m17DemodProcessor.setAudioFifo(&m_audioFifo); m_sampleBuffer = new FixReal[1<<17]; // 128 kS m_sampleBufferIndex = 0; @@ -105,14 +107,13 @@ void M17DemodSink::feed(const SampleVector::const_iterator& begin, const SampleV m_magsqSum += magsq; - if (magsq > m_magsqPeak) - { + if (magsq > m_magsqPeak) { m_magsqPeak = magsq; } m_magsqCount++; - Real demod = m_phaseDiscri.phaseDiscriminator(ci) * m_settings.m_demodGain; // [-1.0:1.0] + Real demod = m_phaseDiscri.phaseDiscriminator(ci); m_sampleCount++; // AF processing @@ -155,12 +156,14 @@ void M17DemodSink::feed(const SampleVector::const_iterator& begin, const SampleV { if (m_squelchGate > 0) { - sampleM17 = m_squelchDelayLine.readBack(m_squelchGate) * 32768.0f; // DSD decoder takes int16 samples + sampleM17 = m_squelchDelayLine.readBack(m_squelchGate) * 32768.0f; // M17 decoder takes int16 samples + m_m17DemodProcessor.pushSample(sampleM17); sample = m_squelchDelayLine.readBack(m_squelchGate) * SDR_RX_SCALEF; // scale to sample size } else { sampleM17 = demod * 32768.0f; // M17 decoder takes int16 samples + m_m17DemodProcessor.pushSample(sampleM17); sample = demod * SDR_RX_SCALEF; // scale to sample size } } @@ -168,9 +171,15 @@ void M17DemodSink::feed(const SampleVector::const_iterator& begin, const SampleV { sampleM17 = 0; sample = 0; + + if (m_squelchWasOpen) + { + m_m17DemodProcessor.resetInfo(); + m_m17DemodProcessor.setDCDOff(); // indicate loss of carrier + } } - // m_dsdDecoder.pushSample(sampleM17); + m_squelchWasOpen = m_squelchOpen; m_demodBuffer[m_demodBufferFill] = sampleM17; ++m_demodBufferFill; @@ -230,39 +239,6 @@ void M17DemodSink::feed(const SampleVector::const_iterator& begin, const SampleV } } - // if (!m_ambeFeature) - // { - // if (m_settings.m_slot1On) - // { - // int nbAudioSamples; - // short *dsdAudio = m_dsdDecoder.getAudio1(nbAudioSamples); - - // if (nbAudioSamples > 0) - // { - // if (!m_settings.m_audioMute) { - // m_audioFifo1.write((const quint8*) dsdAudio, nbAudioSamples); - // } - - // m_dsdDecoder.resetAudio1(); - // } - // } - - // if (m_settings.m_slot2On) - // { - // int nbAudioSamples; - // short *dsdAudio = m_dsdDecoder.getAudio2(nbAudioSamples); - - // if (nbAudioSamples > 0) - // { - // if (!m_settings.m_audioMute) { - // m_audioFifo2.write((const quint8*) dsdAudio, nbAudioSamples); - // } - - // m_dsdDecoder.resetAudio2(); - // } - // } - // } - if ((m_scopeXY != nullptr) && (m_scopeEnabled)) { m_scopeXY->feed(m_scopeSampleBuffer.begin(), m_scopeSampleBuffer.end(), true); // true = real samples for what it's worth @@ -285,7 +261,7 @@ void M17DemodSink::applyAudioSampleRate(int sampleRate) qDebug("M17DemodSink::applyAudioSampleRate: audio will sound best with sample rates that are integer multiples of 8 kS/s"); } - // m_dsdDecoder.setUpsampling(upsampling); + m_m17DemodProcessor.setUpsampling(upsampling); m_audioSampleRate = sampleRate; QList pipes; @@ -332,7 +308,6 @@ void M17DemodSink::applySettings(const M17DemodSettings& settings, bool force) << " m_inputFrequencyOffset: " << settings.m_inputFrequencyOffset << " m_rfBandwidth: " << settings.m_rfBandwidth << " m_fmDeviation: " << settings.m_fmDeviation - << " m_demodGain: " << settings.m_demodGain << " m_volume: " << settings.m_volume << " m_baudRate: " << settings.m_baudRate << " m_squelchGate" << settings.m_squelchGate @@ -355,8 +330,7 @@ void M17DemodSink::applySettings(const M17DemodSettings& settings, bool force) //m_phaseDiscri.setFMScaling((float) settings.m_rfBandwidth / (float) settings.m_fmDeviation); } - if ((settings.m_fmDeviation != m_settings.m_fmDeviation) || force) - { + if ((settings.m_fmDeviation != m_settings.m_fmDeviation) || force) { m_phaseDiscri.setFMScaling(48000.0f / (2.0f*settings.m_fmDeviation)); } @@ -366,15 +340,16 @@ void M17DemodSink::applySettings(const M17DemodSettings& settings, bool force) m_squelchCount = 0; // reset squelch open counter } - if ((settings.m_squelch != m_settings.m_squelch) || force) - { - // input is a value in dB - m_squelchLevel = std::pow(10.0, settings.m_squelch / 10.0); + if ((settings.m_squelch != m_settings.m_squelch) || force) { + m_squelchLevel = std::pow(10.0, settings.m_squelch / 10.0); // input is a value in dB } - if ((settings.m_volume != m_settings.m_volume) || force) - { - // m_dsdDecoder.setAudioGain(settings.m_volume); + if ((settings.m_audioMute != m_settings.m_audioMute) || force) { + m_m17DemodProcessor.setAudioMute(settings.m_audioMute); + } + + if ((settings.m_volume != m_settings.m_volume) || force) { + m_m17DemodProcessor.setVolume(settings.m_volume); } if ((settings.m_baudRate != m_settings.m_baudRate) || force) @@ -382,9 +357,8 @@ void M17DemodSink::applySettings(const M17DemodSettings& settings, bool force) // m_dsdDecoder.setBaudRate(settings.m_baudRate); } - if ((settings.m_highPassFilter != m_settings.m_highPassFilter) || force) - { - // m_dsdDecoder.useHPMbelib(settings.m_highPassFilter); + if ((settings.m_highPassFilter != m_settings.m_highPassFilter) || force) { + m_m17DemodProcessor.setHP(settings.m_highPassFilter); } m_settings = settings; @@ -392,6 +366,7 @@ void M17DemodSink::applySettings(const M17DemodSettings& settings, bool force) void M17DemodSink::configureMyPosition(float myLatitude, float myLongitude) { - // m_dsdDecoder.setMyPoint(myLatitude, myLongitude); + m_latitude = myLatitude; + m_longitude = myLongitude; } diff --git a/plugins/channelrx/demodm17/m17demodsink.h b/plugins/channelrx/demodm17/m17demodsink.h index da8d7d447..3785b6f17 100644 --- a/plugins/channelrx/demodm17/m17demodsink.h +++ b/plugins/channelrx/demodm17/m17demodsink.h @@ -32,6 +32,7 @@ #include "util/doublebufferfifo.h" #include "m17demodsettings.h" +#include "m17demodprocessor.h" class BasebandSampleSink; class ChannelAPI; @@ -75,6 +76,28 @@ public: m_magsqCount = 0; } + void getDiagnostics( + bool& dcd, + float& evm, + float& deviation, + float& offset, + int& status, + float& clock, + int& sampleIndex, + int& syncIndex, + int& clockIndex, + int& viterbiCost + ) const + { + m_m17DemodProcessor.getDiagnostics(dcd, evm, deviation, offset, status, clock, sampleIndex, syncIndex, clockIndex, viterbiCost); + } + + uint32_t getLSFCount() const { return m_m17DemodProcessor.getLSFCount(); } + const QString& getSrcCall() const { return m_m17DemodProcessor.getSrcCall(); } + const QString& getDestcCall() const { return m_m17DemodProcessor.getDestcCall(); } + const QString& getTypeInfo() const { return m_m17DemodProcessor.getTypeInfo(); } + uint16_t getCRC() const { return m_m17DemodProcessor.getCRC(); } + private: struct MagSqLevelsStore { @@ -108,6 +131,7 @@ private: int m_squelchGate; double m_squelchLevel; bool m_squelchOpen; + bool m_squelchWasOpen; DoubleBufferFIFO m_squelchDelayLine; MovingAverageUtil m_movingAverage; @@ -128,7 +152,12 @@ private: BasebandSampleSink* m_scopeXY; bool m_scopeEnabled; + float m_latitude; + float m_longitude; + PhaseDiscriminators m_phaseDiscri; + + M17DemodProcessor m_m17DemodProcessor; }; #endif // INCLUDE_DSDDEMODSINK_H