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