diff --git a/modemm17/FreqDevEstimator.cpp b/modemm17/FreqDevEstimator.cpp index 1c3d1c36a..1242adbf6 100644 --- a/modemm17/FreqDevEstimator.cpp +++ b/modemm17/FreqDevEstimator.cpp @@ -4,5 +4,6 @@ namespace modemm17 { const std::array FreqDevEstimator::dc_b = { 0.09763107, 0.19526215, 0.09763107 }; const std::array FreqDevEstimator::dc_a = { 1. , -0.94280904, 0.33333333 }; +const float FreqDevEstimator::MAX_DC_ERROR = 0.2f; } // namespace modemm17 diff --git a/modemm17/FreqDevEstimator.h b/modemm17/FreqDevEstimator.h index 8b5c0119c..c5a4aa0c7 100644 --- a/modemm17/FreqDevEstimator.h +++ b/modemm17/FreqDevEstimator.h @@ -28,40 +28,19 @@ namespace modemm17 { */ class MODEMM17_API FreqDevEstimator { - using sample_filter_t = BaseIirFilter<3>; - - // IIR with Nyquist of 1/4. - static const std::array dc_b; - static const std::array dc_a; - - static constexpr float MAX_DC_ERROR = 0.2; - - float min_est_ = 0.0; - float max_est_ = 0.0; - float min_cutoff_ = 0.0; - float max_cutoff_ = 0.0; - float min_var_ = 0.0; - float max_var_ = 0.0; - size_t min_count_ = 0; - size_t max_count_ = 0; - float deviation_ = 0.0; - float offset_ = 0.0; - float error_ = 0.0; - float idev_ = 1.0; - sample_filter_t dc_filter_{dc_b, dc_a}; - public: + using sample_filter_t = BaseIirFilter<3>; void reset() { - min_est_ = 0.0; - max_est_ = 0.0; - min_var_ = 0.0; - max_var_ = 0.0; + min_est_ = 0.0f; + max_est_ = 0.0f; + min_var_ = 0.0f; + max_var_ = 0.0f; min_count_ = 0; max_count_ = 0; - min_cutoff_ = 0.0; - max_cutoff_ = 0.0; + min_cutoff_ = 0.0f; + max_cutoff_ = 0.0f; } void sample(float sample) @@ -70,8 +49,8 @@ public: { min_count_ = 1; min_est_ = sample; - min_var_ = 0.0; - min_cutoff_ = min_est_ * 0.666666; + min_var_ = 0.0f; + min_cutoff_ = min_est_ * 0.666666f; } else if (sample < min_cutoff_) { @@ -80,12 +59,12 @@ public: float var = (min_est_ / min_count_) - sample; min_var_ += var * var; } - else if (sample > 1.5 * max_est_) + else if (sample > 1.5f * max_est_) { max_count_ = 1; max_est_ = sample; - max_var_ = 0.0; - max_cutoff_ = max_est_ * 0.666666; + max_var_ = 0.0f; + max_cutoff_ = max_est_ * 0.666666f; } else if (sample > max_cutoff_) { @@ -104,27 +83,54 @@ public: */ void update() { - if (max_count_ < 2 || min_count_ < 2) return; + if (max_count_ < 2 || min_count_ < 2) { + return; + } + float max_ = max_est_ / max_count_; float 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; + + 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; + max_var_ = 0.0f; + min_var_ = 0.0f; } float deviation() const { return deviation_; } float offset() const { return offset_; } float error() const { return error_; } float idev() const { return idev_; } + +private: + // IIR with Nyquist of 1/4. + static const std::array dc_b; + static const std::array dc_a; + static const float MAX_DC_ERROR; + + float min_est_ = 0.0f; + float max_est_ = 0.0f; + float min_cutoff_ = 0.0f; + float max_cutoff_ = 0.0f; + float min_var_ = 0.0f; + float max_var_ = 0.0f; + size_t min_count_ = 0; + size_t max_count_ = 0; + float deviation_ = 0.0f; + float offset_ = 0.0f; + float error_ = 0.0f; + float idev_ = 1.0f; + sample_filter_t dc_filter_{dc_b, dc_a}; }; } // modemm17