kopia lustrzana https://github.com/mobilinkd/NucleoTNC
Add new components for updated M17 demodulator.
Rob Riggs
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