From bc1a6f3a854a0058f9ede043e1f3521197665d8b Mon Sep 17 00:00:00 2001 From: Mark Date: Fri, 24 Jun 2022 21:28:03 -0700 Subject: [PATCH] Added FFT code --- src/signal_processing/fft.cpp | 313 ++++++++++++++++++++++++++++++++++ src/signal_processing/fft.hpp | 121 +++++++++++++ 2 files changed, 434 insertions(+) create mode 100644 src/signal_processing/fft.cpp create mode 100644 src/signal_processing/fft.hpp diff --git a/src/signal_processing/fft.cpp b/src/signal_processing/fft.cpp new file mode 100644 index 0000000..ebf1a1a --- /dev/null +++ b/src/signal_processing/fft.cpp @@ -0,0 +1,313 @@ +#include "fft.hpp" + + +FFT_Visualizer::FFT_Visualizer( + size_t width, + size_t height, + + // Advanced options. These have default values. + double HZ_MIN, + double HZ_MAX, + uint32_t DFT_TOTAL_SIZE, + uint32_t DFT_NONZERO_SIZE, + double GAIN +): + width(width), + height(height), + HZ_MIN(HZ_MIN), + HZ_MAX(HZ_MAX), + DFT_TOTAL_SIZE(DFT_TOTAL_SIZE), + DFT_NONZERO_SIZE(DFT_NONZERO_SIZE), + GAIN(GAIN), + DYNAMIC_RANGE(100 - GAIN) +{ + + fftw_results = DFT_TOTAL_SIZE/2 + 1; + + freq_magnitudes.resize(fftw_results); + partial_output.reserve(width); + output.resize(width); + GenLogspace(); + + fftw_input = static_cast(fftw_malloc(sizeof(double)*DFT_TOTAL_SIZE)); + fftw_output = static_cast(fftw_malloc(sizeof(fftw_complex)*fftw_results)); + memset(fftw_input, 0, sizeof(double)*DFT_TOTAL_SIZE); + + plan = fftw_plan_dft_r2c_1d( + DFT_TOTAL_SIZE, + fftw_input, + fftw_output, + FFTW_ESTIMATE + ); +} + + +FFT_Visualizer::~FFT_Visualizer() { + fftw_destroy_plan(plan); + fftw_free(fftw_input); + fftw_free(fftw_output); +} + + +/* +Returns ideal size for this visualizer's Buffer's output. +*/ +size_t FFT_Visualizer::compute_buffer_output_size() const { + return DFT_NONZERO_SIZE; +} + + +// Generate log-scaled vector of frequencies from HZ_MIN to HZ_MAX. +void FFT_Visualizer::GenLogspace() { + // Prepare vector + logspace.resize(width); + + // Calculate number of extra bins needed between 0 HZ and HZ_MIN + // + // In logspace, divide the region between MAX and MIN into + // `width - 1` equal segments (fenceposts; this gives us `width` seperators) + const double d = ( + (log10(HZ_MAX) - log10(HZ_MIN)) + / + (width - 1) + ); + // Count how many of these segments will fit between + // 0 and MIN (note that we're still in logspace). + // This is how many log-scaled intervals are outside + // our desired range of frequencies. + const size_t skip_bins = log10(HZ_MIN) / d; + + // Calculate log scale size. + // We can't use the value of d here, because d is "anchored" to both MIN and MAX. + // The last bin should be equal to MAX, but there may not be a bin that is equal to MIN. + // + // So, we re-partition our logspace: + // Divide the distance between 0 and MAX into equal partitions. + const double log_scale = log10(HZ_MAX) / (skip_bins + width - 1); + + // Exponential-map bins out of logspace, skipping those that are outside our range. + // Note that the first (skipped) bin is ALWAYS 1, since 10^(0 * log_scale) = 1. + // The last bin ALWAYS equals MAX. + for (size_t i = skip_bins; i < width + skip_bins; ++i) { + logspace[i - skip_bins] = pow(10, i * log_scale); + } +} + + + +/* +Apply the Blackman window and copy buffer data. + +@param output Where to write output data +@param input Raw data from buffer +@param data_length How much data there is. The lengths + of both input and output must be equal to this value. +*/ +void FFT_Visualizer::ApplyWindow( + double* output, + const int16_t* input, + ssize_t data_length +) const { + // samples = length of input = length of output + + // Constants. + // These give us low sidelobes and fast sidelobe rolloff. + // https://en.wikipedia.org/wiki/Window_function#Blackman_window + // + // See also: + // https://en.wikipedia.org/wiki/Spectral_leakage + const double alpha = 0.16; + const double a0 = (1 - alpha) / 2; + const double a1 = 0.5; + const double a2 = alpha / 2; + const double pi = 3.151592653; + + const double window_width = DFT_NONZERO_SIZE - 1; + + for (unsigned i = 0; i < data_length; ++i) { + double window = ( + a0 - + a1*cos( + 2*pi*i / + window_width + ) + + a2*cos( + 4*pi*i / + window_width + ) + ); + + // Output values are between 1 and -1. + output[i] = (window * input[i]) / INT16_MAX; + } +} + + + +/* +Not sure what this does yet. + +Takes values in [0, DFT_TOTAL_SIZE/2], +returns values in [0, 44100 / 2]. + +@param bin An index of `freq_magnitudes` +*/ +double FFT_Visualizer::Bin2Hz(size_t bin) const { + + return bin*44100/DFT_TOTAL_SIZE; +} + + + +/* +Fill empty bins with interpolated data. + +@param target_x The index of the bin we want to fill. +@param last_bar_idx The index of the last non-empty and non-interpolated bin. +*/ +double FFT_Visualizer::Interpolate( + size_t target_x, + size_t last_bar_idx +) { + const double x_next = partial_output[last_bar_idx].first; + const double h_next = partial_output[last_bar_idx].second; + + double dh = 0; + if (last_bar_idx == 0) { + // no data points on left, linear extrapolation + if (last_bar_idx < partial_output.size()-1) { + const double x_next2 = partial_output[last_bar_idx + 1].first; + const double h_next2 = partial_output[last_bar_idx + 1].second; + dh = (h_next2 - h_next) / (x_next2 - x_next); + } + return h_next - dh*(x_next - target_x); + + } else if (last_bar_idx == 1) { + // one data point on left, linear interpolation + const double x_prev = partial_output[last_bar_idx - 1].first; + const double h_prev = partial_output[last_bar_idx - 1].second; + dh = (h_next - h_prev) / (x_next - x_prev); + return h_next - dh*(x_next - target_x); + + } else if (last_bar_idx < partial_output.size() - 1) { + // Two data points on both sides, cubic interpolation + // https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Interpolation_on_an_arbitrary_interval + const double x_prev2 = partial_output[last_bar_idx - 2].first; + const double h_prev2 = partial_output[last_bar_idx - 2].second; + const double x_prev = partial_output[last_bar_idx - 1].first; + const double h_prev = partial_output[last_bar_idx - 1].second; + const double x_next2 = partial_output[last_bar_idx + 1].first; + const double h_next2 = partial_output[last_bar_idx + 1].second; + + const double m0 = (h_prev - h_prev2) / (x_prev - x_prev2); + const double m1 = (h_next2 - h_next) / (x_next2 - x_next); + const double t = (target_x - x_prev) / (x_next - x_prev); + const double h00 = 2*t*t*t - 3*t*t + 1; + const double h10 = t*t*t - 2*t*t + t; + const double h01 = -2*t*t*t + 3*t*t; + const double h11 = t*t*t - t*t; + + return h00*h_prev + h10*(x_next-x_prev)*m0 + h01*h_next + h11*(x_next-x_prev)*m1; + } + + // Less than two data points on right, no interpolation. + // This should never happen unless you have a VERY low DFT size + return h_next; +} + + + +/* +Read a buffer and update output array +*/ +void FFT_Visualizer::update( + const Buffer& buf +) { + + // Load data and execute FFT + ApplyWindow(fftw_input, buf.get_output().data(), buf.get_output().size()); + fftw_execute(plan); + + + // Count magnitude of each frequency and normalize + // (fftw does not normalize) + for (size_t i = 0; i < fftw_results; ++i) { + freq_magnitudes[i] = sqrt( + fftw_output[i][0]*fftw_output[i][0] + + + fftw_output[i][1]*fftw_output[i][1] + ) / (DFT_NONZERO_SIZE); + } + + + // Skip magnitudes not in log space + size_t cur_bin = 0; + while (cur_bin < fftw_results && Bin2Hz(cur_bin) < logspace[0]) { + ++cur_bin; + } + + + + partial_output.clear(); + + // Accumulate magnitudes into bins + for (size_t x = 0; x < width; x++) { + double bar_height = 0; + size_t count = 0; + + // Check right bound + while (cur_bin < fftw_results && Bin2Hz(cur_bin) < logspace[x]) { + // Check left bound if not first index + if (x == 0 || Bin2Hz(cur_bin) >= logspace[x-1]) { + bar_height += freq_magnitudes[cur_bin]; + ++count; + } + ++cur_bin; + } + + // If bin is empty, we're done. + // Don't add this bin to partial_output, + // it will be interpolated later. + if (count == 0) { + continue; + } + + // average bins + bar_height /= count; + + // log scale heights + bar_height = (20 * log10(bar_height) + DYNAMIC_RANGE + GAIN) / DYNAMIC_RANGE; + // Scale bar height between 0 and height + bar_height = bar_height > 0 ? bar_height * (height-1) : 0; + bar_height = bar_height >= height ? (height-1) : bar_height; + + + // use emplace_back to prevent redundant copy operations. + // faster than push_back, in this case. + partial_output.emplace_back(x, bar_height); + } + + size_t bar_idx = 0; + for (size_t x = 0; x < width; x++) { + const size_t bar_x = partial_output[bar_idx].first; + const double bar_height = partial_output[bar_idx].second; + + if (x == bar_x) { + // This data point exists, add it to output array. + output[x] = (size_t) bar_height; + if (bar_idx < partial_output.size() - 1) { + bar_idx++; + } + } else { + // data point does not exist, we need to interpolate + + // This check shouldn't be necessary, but sometimes + // Interpolate throws out a negative value. + // + // figure out why! + double i = Interpolate(x, bar_idx); + + output[x] = (size_t) (i > 0 ? i : 0); + } + } +} \ No newline at end of file diff --git a/src/signal_processing/fft.hpp b/src/signal_processing/fft.hpp new file mode 100644 index 0000000..738f7ec --- /dev/null +++ b/src/signal_processing/fft.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include +#include +#include + +#include +#include // memset +#include + +#include "utility/buffer.hpp" + +class FFT_Visualizer { + public: + FFT_Visualizer( + size_t width, + size_t height, + + // Advanced options you probably shouldn't touch + double HZ_MIN = 20, + double HZ_MAX = 20000, + uint32_t DFT_TOTAL_SIZE = 1 << 15, + // #define conf_spectrum_dft_size 2 (between 1 and 5, inclusive) + uint32_t DFT_NONZERO_SIZE = (2048 * (2*2 + 4)), + double GAIN = 10 + ); + + ~FFT_Visualizer(); + + void update( + const Buffer& buf + ); + + size_t compute_buffer_output_size() const; + + const std::vector& get_output() { + return output; + }; + + private: + + /// + // Visualizer parameters + /// + + // How many bars this visualizer will generate + const size_t width; + // Resolution of this visualizer's bars. + const size_t height; + // Leftmost frequency in spectrum + const double HZ_MIN; + // Rightmost frequency in spectrum + const double HZ_MAX; + // Not sure what this does + const uint32_t DFT_TOTAL_SIZE; + // Not sure what this does, either + const uint32_t DFT_NONZERO_SIZE; + // Visualization spectrum gain + // tune if bars are too small + const double GAIN; + // Not sure what this does + const double DYNAMIC_RANGE; + + + /// + // FFTW resources + /// + + fftw_plan plan; + // How many output values fftw will give us + size_t fftw_results; + // Input array. This is filled with buffer data + // that has been filtered through a window. + double* fftw_input; + // FFT output array. + fftw_complex *fftw_output; + + /// + // Intermediate values + /// + + // The magnitudes of the complex values fftw returns. + std::vector freq_magnitudes; + // An array of frequency bins, scaled logarithmically. + std::vector logspace; + // Nearly-complete output. + // This is necessary because some frequency bins + // may be empty, and need to be interpolated + std::vector< + std::pair< + size_t, + double + > + > partial_output; + + + // Output vector, with empty bins interpolated. + // The maximum possible value of an element in this + // vector is `height`. + std::vector output; + + + /// + // Helper methods + // See definition for docs. + /// + + void GenLogspace(); + double Bin2Hz(size_t bin) const; + + void ApplyWindow( + double *output, + const int16_t *input, + ssize_t samples + ) const; + + double Interpolate( + size_t x, + size_t h_idx + ); +}; \ No newline at end of file