You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
62 lines
1.1 KiB
62 lines
1.1 KiB
// Copyright 2020 Mobilinkd LLC. |
|
|
|
#pragma once |
|
|
|
#include "IirFilter.h" |
|
|
|
#include <array> |
|
#include <algorithm> |
|
#include <numeric> |
|
|
|
namespace modemm17 |
|
{ |
|
|
|
template <size_t N = 32> |
|
struct FrequencyError |
|
{ |
|
FrequencyError() : |
|
{ |
|
samples_.fill(0.0f); |
|
} |
|
|
|
auto operator()(float sample) |
|
{ |
|
float 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); |
|
} |
|
|
|
private: |
|
static constexpr std::array<float, 3> evm_b{0.02008337, 0.04016673, 0.02008337}; |
|
static constexpr std::array<float, 3> evm_a{1.0, -1.56101808, 0.64135154}; |
|
|
|
std::array<float, N> samples_; |
|
size_t index_ = 0; |
|
float accum_ = 0.0f; |
|
BaseIirFilter<3> filter_{makeIirFilter(evm_b, evm_a)}; |
|
|
|
const float ZERO = 0.0f; |
|
}; |
|
|
|
} // modemm17
|
|
|