2022-07-20 03:07:00 -04:00
|
|
|
// Copyright 2021 Mobilinkd LLC.
|
2022-06-06 21:22:18 -04:00
|
|
|
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <array>
|
|
|
|
#include <cmath>
|
|
|
|
#include <complex>
|
|
|
|
#include <cstddef>
|
|
|
|
|
2022-07-04 17:03:07 -04:00
|
|
|
namespace modemm17
|
2022-06-06 21:22:18 -04:00
|
|
|
{
|
|
|
|
|
|
|
|
/**
|
|
|
|
* A sliding DFT algorithm.
|
|
|
|
*
|
|
|
|
* Based on 'Understanding and Implementing the Sliding DFT'
|
|
|
|
* Eric Jacobsen, 2015-04-23
|
|
|
|
* https://www.dsprelated.com/showarticle/776.php
|
|
|
|
*/
|
2022-06-09 14:12:35 -04:00
|
|
|
template <size_t SampleRate, size_t Frequency, size_t Accuracy = 1000>
|
2022-06-06 21:22:18 -04:00
|
|
|
class SlidingDFT
|
|
|
|
{
|
2022-07-27 12:53:40 -04:00
|
|
|
public:
|
2022-06-09 14:12:35 -04:00
|
|
|
using ComplexType = std::complex<float>;
|
2022-06-06 21:22:18 -04:00
|
|
|
|
|
|
|
SlidingDFT()
|
|
|
|
{
|
|
|
|
samples_.fill(0);
|
2022-07-27 12:53:40 -04:00
|
|
|
float pi2 = M_PI * 2.0f;
|
|
|
|
float kth = float(Frequency) / float(SampleRate);
|
2022-06-06 21:22:18 -04:00
|
|
|
coeff_ = std::exp(-ComplexType{0, 1} * pi2 * kth);
|
|
|
|
}
|
|
|
|
|
2022-06-09 14:12:35 -04:00
|
|
|
ComplexType operator()(float sample)
|
2022-06-06 21:22:18 -04:00
|
|
|
{
|
|
|
|
auto index = index_;
|
|
|
|
index_ += 1;
|
2022-07-27 12:53:40 -04:00
|
|
|
if (index_ == (SampleRate / Accuracy)) index_ = 0;
|
2022-06-06 21:22:18 -04:00
|
|
|
|
2022-06-09 14:12:35 -04:00
|
|
|
float delta = sample - samples_[index];
|
2022-06-06 21:22:18 -04:00
|
|
|
ComplexType result = (result_ + delta) * coeff_;
|
2022-06-09 14:12:35 -04:00
|
|
|
result_ = result * float(0.999999999999999);
|
2022-06-06 21:22:18 -04:00
|
|
|
samples_[index] = sample;
|
|
|
|
prev_index_ = index;
|
|
|
|
return result;
|
|
|
|
}
|
2022-07-27 12:53:40 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
ComplexType coeff_;
|
|
|
|
std::array<float, (SampleRate / Accuracy)> samples_;
|
|
|
|
ComplexType result_{0,0};
|
|
|
|
size_t index_ = 0;
|
|
|
|
size_t prev_index_ = (SampleRate / Accuracy) - 1;
|
2022-06-06 21:22:18 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
* A sliding DFT algorithm.
|
|
|
|
*
|
|
|
|
* Based on 'Understanding and Implementing the Sliding DFT'
|
|
|
|
* Eric Jacobsen, 2015-04-23
|
|
|
|
* https://www.dsprelated.com/showarticle/776.php
|
|
|
|
*
|
2022-06-09 14:12:35 -04:00
|
|
|
* @tparam float is the floating point type to use.
|
2022-06-06 21:22:18 -04:00
|
|
|
* @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.
|
|
|
|
*/
|
2022-06-09 14:12:35 -04:00
|
|
|
template <size_t SampleRate, size_t N, size_t K>
|
2022-06-06 21:22:18 -04:00
|
|
|
class NSlidingDFT
|
|
|
|
{
|
|
|
|
public:
|
2022-07-27 12:53:40 -04:00
|
|
|
using ComplexType = std::complex<float>;
|
2022-06-06 21:22:18 -04:00
|
|
|
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.
|
|
|
|
*/
|
2022-06-09 14:12:35 -04:00
|
|
|
result_type operator()(float sample)
|
2022-06-06 21:22:18 -04:00
|
|
|
{
|
|
|
|
auto index = index_;
|
|
|
|
index_ += 1;
|
|
|
|
if (index_ == N) index_ = 0;
|
|
|
|
|
2022-06-09 14:12:35 -04:00
|
|
|
float delta = sample - samples_[index];
|
2022-06-06 21:22:18 -04:00
|
|
|
|
|
|
|
for (size_t i = 0; i != K; ++i)
|
|
|
|
{
|
|
|
|
result_[i] = (result_[i] + delta) * coeff_[i];
|
|
|
|
}
|
|
|
|
samples_[index] = sample;
|
|
|
|
return result_;
|
|
|
|
}
|
2022-07-27 12:53:40 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
const std::array<ComplexType, K> coeff_;
|
|
|
|
std::array<float, 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)
|
|
|
|
{
|
|
|
|
ComplexType j = ComplexType{0, 1};
|
|
|
|
std::array<ComplexType, K> result;
|
|
|
|
float pi2 = M_PI * 2.0f;
|
|
|
|
for (size_t i = 0; i != K; ++i)
|
|
|
|
{
|
|
|
|
float k = float(frequencies[i]) / float(SampleRate);
|
|
|
|
result[i] = std::exp(-j * pi2 * k);
|
|
|
|
}
|
|
|
|
return result;
|
|
|
|
}
|
2022-06-06 21:22:18 -04:00
|
|
|
};
|
|
|
|
|
2022-07-04 17:03:07 -04:00
|
|
|
} // modemm17
|