kopia lustrzana https://github.com/mobilinkd/m17-cxx-demod
57 wiersze
1.3 KiB
C++
57 wiersze
1.3 KiB
C++
// Copyright 2021 Rob Riggs <rob@mobilinkd.com>
|
|
// All rights reserved.
|
|
|
|
#pragma once
|
|
|
|
#include "KalmanFilter.h"
|
|
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
|
|
namespace mobilinkd {
|
|
|
|
template <typename FloatType>
|
|
class FreqDevEstimator
|
|
{
|
|
static constexpr FloatType DEVIATION = 2400.;
|
|
|
|
m17::SymbolKalmanFilter<FloatType> minFilter_;
|
|
m17::SymbolKalmanFilter<FloatType> maxFilter_;
|
|
FloatType idev_ = 0.;
|
|
FloatType offset_ = 0.;
|
|
bool reset_ = true;
|
|
|
|
public:
|
|
|
|
void reset()
|
|
{
|
|
reset_ = true;
|
|
}
|
|
|
|
void update(FloatType minValue, FloatType maxValue)
|
|
{
|
|
auto mnf = minFilter_.update(minValue, 192);
|
|
auto mxf = maxFilter_.update(maxValue, 192);
|
|
offset_ = (mxf[0] + mnf[0]) / 2.;
|
|
idev_ = 6.0 / (mxf[0] - mnf[0]);
|
|
|
|
if (isnan(mnf) || isnan(mxf)) reset_ = true;
|
|
|
|
if (reset_)
|
|
{
|
|
reset_ = false;
|
|
minFilter_.reset(minValue);
|
|
maxFilter_.reset(maxValue);
|
|
offset_ = (minValue + maxValue) / 2;
|
|
idev_ = 6.0 / (maxValue - minValue);
|
|
}
|
|
}
|
|
|
|
FloatType idev() const { return idev_; }
|
|
FloatType offset() const { return offset_; }
|
|
FloatType deviation() const { return DEVIATION / idev_; }
|
|
FloatType error() const { return 0.; }
|
|
};
|
|
|
|
} // mobilinkd
|