Skip to content

Commit

Permalink
more debug + optimize fft
Browse files Browse the repository at this point in the history
  • Loading branch information
rtlopez committed Aug 3, 2023
1 parent ee22bf6 commit 0a01532
Show file tree
Hide file tree
Showing 4 changed files with 116 additions and 47 deletions.
6 changes: 3 additions & 3 deletions lib/Espfc/src/Espfc.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,10 @@ class Espfc
int update()
{
#if defined(ESPFC_MULTI_CORE)
if(!_model.state.appQueue.isEmpty())
/*if(!_model.state.appQueue.isEmpty())
{
return 0;
}
}*/

if(!_model.state.gyroTimer.check())
{
Expand All @@ -82,7 +82,7 @@ class Espfc
_buzzer.update();
_model.state.stats.update();
}
_model.state.appQueue.send(Event(EVENT_IDLE));
//_model.state.appQueue.send(Event(EVENT_IDLE));

return 1;
#else
Expand Down
31 changes: 28 additions & 3 deletions lib/Espfc/src/Input.h
Original file line number Diff line number Diff line change
Expand Up @@ -120,9 +120,15 @@ class Input
InputStatus readInputs()
{
Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ);
uint32_t startTime = micros();

InputStatus status = _device->update();

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[0] = micros() - startTime;
}

if(status == INPUT_IDLE) return status;

_model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE);
Expand All @@ -148,6 +154,8 @@ class Input
{
if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage

uint32_t startTime = micros();

uint16_t channels[INPUT_CHANNELS];
_device->get(channels, _model.state.inputChannelCount);

Expand Down Expand Up @@ -187,6 +195,11 @@ class Input
_model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c];
_model.state.inputBuffer[c] = v;
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[2] = micros() - startTime;
}
}

bool failsafe(InputStatus status)
Expand Down Expand Up @@ -259,6 +272,7 @@ class Input
void filterInputs(InputStatus status)
{
Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER);
uint32_t startTime = micros();

const bool newFrame = status != INPUT_IDLE;
const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION;
Expand All @@ -284,6 +298,11 @@ class Input
}
setInput((Axis)c, v, newFrame);
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[3] = micros() - startTime;
}
}

void updateFrameRate()
Expand All @@ -303,7 +322,8 @@ class Input

if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
{
_model.state.debug[0] = _model.state.inputFrameRate;
_model.state.debug[0] = _model.state.inputFrameDelta / 10;
_model.state.debug[1] = _model.state.inputFrameRate;
}

// auto cutoff input freq
Expand All @@ -313,8 +333,8 @@ class Input
_model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq);
if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
{
_model.state.debug[1] = lrintf(freq);
_model.state.debug[2] = lrintf(_model.state.inputAutoFreq);
_model.state.debug[2] = lrintf(freq);
_model.state.debug[3] = lrintf(_model.state.inputAutoFreq);
}
FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq);
FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq);
Expand All @@ -330,6 +350,11 @@ class Input
}
}
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[1] = micros() - now;
}
}

Device::InputDevice * getInputDevice()
Expand Down
114 changes: 76 additions & 38 deletions lib/Espfc/src/Math/FFTAnalyzer.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,89 +7,120 @@
#include "Filter.h"
#include "dsps_fft4r.h"
#include "dsps_wind_hann.h"
#include <algorithm>

namespace Espfc {

namespace Math {

enum FFTPhase {
PHASE_COLLECT,
PHASE_FFT,
PHASE_PEAKS
};

template<size_t SAMPLES>
class FFTAnalyzer
{
public:
FFTAnalyzer(): _idx(0) {}
FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT) {}

int begin(int16_t rate, const DynamicFilterConfig& config)
int begin(int16_t rate, const DynamicFilterConfig& config, size_t axis)
{
int16_t nyquistLimit = rate / 2;
_rate = rate;
_freq_min = config.min_freq;
_freq_max = std::min(config.max_freq, nyquistLimit);
_peak_count = std::min((size_t)config.width, PEAKS_MAX);

_idx = 0;
_idx = axis * SAMPLES / 3;
_bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results

dsps_fft4r_init_fc32(NULL, BINS);
_begin = (_freq_min / _bin_width) + 1;
_end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1;

// init fft tables
dsps_fft4r_init_fc32(nullptr, BINS);

// Generate hann window
dsps_wind_hann_f32(_wind, SAMPLES);
dsps_wind_hann_f32(_win, SAMPLES);

clearPeaks();

for(size_t i = 0; i < SAMPLES; i++) _in[i] = 0;
//std::fill(_in, _in + SAMPLES, 0);

return 1;
}

// calculate fft and find noise peaks
int update(float v)
{
_samples[_idx] = v;

if(++_idx < SAMPLES) return 0; // not enough samples
_in[_idx] = v;

_idx = 0;

// apply window function
for (size_t j = 0; j < SAMPLES; j++)
if(++_idx >= SAMPLES)
{
_samples[j] *= _wind[j]; // real
_idx = 0;
_phase = PHASE_FFT;
}

// FFT Radix-4
dsps_fft4r_fc32(_samples, BINS);
switch(_phase)
{
case PHASE_COLLECT:
return 0;

// Bit reverse
dsps_bit_rev4r_fc32(_samples, BINS);
case PHASE_FFT: // 32us
// apply window function
for (size_t j = 0; j < SAMPLES; j++)
{
_out[j] = _in[j] * _win[j]; // real
}

// Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2
dsps_cplx2real_fc32(_samples, BINS);
// FFT Radix-4
dsps_fft4r_fc32(_out, BINS);

// calculate magnitude
for (size_t j = 0; j < BINS; j++)
{
size_t k = j * 2;
_samples[j] = _samples[k] * _samples[k] + _samples[k + 1] * _samples[k + 1];
//_samples[j] = sqrt(_samples[j]);
}
// Bit reverse
dsps_bit_rev4r_fc32(_out, BINS);

clearPeaks();
const size_t begin = (_freq_min / _bin_width) + 1;
const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1;
// Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2
dsps_cplx2real_fc32(_out, BINS);

Math::peakDetect(_samples, begin, end, _bin_width, peaks, _peak_count);
_phase = PHASE_PEAKS;
return 0;

// sort peaks by freq
Math::peakSort(peaks, _peak_count);

return 1;
case PHASE_PEAKS: // 12us + 22us sqrt()
// calculate magnitude
for (size_t j = 0; j < BINS; j++)
{
size_t k = j * 2;
//_out[j] = _out[k] * _out[k] + _out[k + 1] * _out[k + 1];
_out[j] = sqrt(_out[k] * _out[k] + _out[k + 1] * _out[k + 1]);
}

clearPeaks();

Math::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count);

// sort peaks by freq
Math::peakSort(peaks, _peak_count);

_phase = PHASE_COLLECT;
return 1;

default:
_phase = PHASE_COLLECT;
return 0;
}
}

static const size_t PEAKS_MAX = 8;
Peak peaks[PEAKS_MAX];

private:
void clearPeaks()
{
for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Peak();
//std::fill(peaks, peaks + PEAKS_MAX, Peak());
}

static const size_t BINS = SAMPLES >> 1;
Expand All @@ -100,12 +131,19 @@ class FFTAnalyzer
int16_t _peak_count;

size_t _idx;
FFTPhase _phase;
size_t _begin;
size_t _end;
float _bin_width;

// fft input and output
__attribute__((aligned(16))) float _samples[SAMPLES];
// fft input
__attribute__((aligned(16))) float _in[SAMPLES];

// fft output
__attribute__((aligned(16))) float _out[SAMPLES];

// Window coefficients
__attribute__((aligned(16))) float _wind[SAMPLES];
__attribute__((aligned(16))) float _win[SAMPLES];
};

}
Expand Down
12 changes: 9 additions & 3 deletions lib/Espfc/src/Sensor/GyroSensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class GyroSensor: public BaseSensor
#ifdef ESPFC_DSP
for(size_t i = 0; i < 3; i++)
{
_fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter);
_fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter, i);
}
#endif

Expand Down Expand Up @@ -166,7 +166,7 @@ class GyroSensor: public BaseSensor
{
bool dynamicFilterEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER);
bool dynamicFilterFeed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0;
bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ;
bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME;
bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check();
const float q = _model.config.dynamicFilter.q * 0.01;

Expand All @@ -180,11 +180,17 @@ class GyroSensor: public BaseSensor
const size_t peakCount = _model.config.dynamicFilter.width;
if(dynamicFilterFeed)
{
uint32_t startTime = micros();
int status = _fft[i].update(_model.state.gyroDynNotch[i]);
if(_model.config.debugMode == DEBUG_FFT_TIME)
{
if(i == 0) _model.state.debug[0] = status;
_model.state.debug[i + 1] = micros() - startTime;
}
dynamicFilterUpdate = dynamicFilterEnabled && status;
if(dynamicFilterDebug)
{
if(i == _model.config.debugAxis)
if(_model.config.debugMode == DEBUG_FFT_FREQ && i == _model.config.debugAxis)
{
_model.state.debug[0] = lrintf(_fft[i].peaks[0].freq);
_model.state.debug[1] = lrintf(_fft[i].peaks[1].freq);
Expand Down

0 comments on commit 0a01532

Please sign in to comment.