diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 59b2b376..49f60990 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -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()) { @@ -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 diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 09af602e..a590e945 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -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); @@ -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); @@ -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) @@ -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; @@ -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() @@ -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 @@ -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); @@ -330,6 +350,11 @@ class Input } } } + + if(_model.config.debugMode == DEBUG_RX_TIMING) + { + _model.state.debug[1] = micros() - now; + } } Device::InputDevice * getInputDevice() diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index 613b39d4..dab9c6e4 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -7,18 +7,25 @@ #include "Filter.h" #include "dsps_fft4r.h" #include "dsps_wind_hann.h" +#include namespace Espfc { namespace Math { +enum FFTPhase { + PHASE_COLLECT, + PHASE_FFT, + PHASE_PEAKS +}; + template 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; @@ -26,63 +33,86 @@ class FFTAnalyzer _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]; @@ -90,6 +120,7 @@ class FFTAnalyzer 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; @@ -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]; }; } diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 48c3df53..dae7648b 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -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 @@ -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; @@ -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);