From 893c374447e14072ed7b34e8d4f3260e2960b154 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 21:16:23 +0100 Subject: [PATCH] move rpm and dynamic filter to gyro config --- lib/Espfc/src/Blackbox/Blackbox.cpp | 24 +++++++++---------- lib/Espfc/src/Cli.h | 24 +++++++++---------- lib/Espfc/src/Model.h | 12 +++++----- lib/Espfc/src/ModelConfig.h | 13 +++++------ lib/Espfc/src/Msp/MspProcessor.h | 24 +++++++++---------- lib/Espfc/src/Sensor/GyroSensor.cpp | 36 ++++++++++++++--------------- 6 files changed, 66 insertions(+), 67 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 5a91346..a7250ec 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -124,10 +124,10 @@ int Blackbox::begin() gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.freq; gyroConfigMutable()->gyro_sync_denom = 1; - dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; - dynNotchConfigMutable()->dyn_notch_q = _model.config.dynamicFilter.q; - dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.dynamicFilter.min_freq; - dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.dynamicFilter.max_freq; + dynNotchConfigMutable()->dyn_notch_count = _model.config.gyro.dynamicFilter.width; + dynNotchConfigMutable()->dyn_notch_q = _model.config.gyro.dynamicFilter.q; + dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.gyro.dynamicFilter.min_freq; + dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.gyro.dynamicFilter.max_freq; accelerometerConfigMutable()->acc_lpf_hz = _model.config.accel.filter.freq; accelerometerConfigMutable()->acc_hardware = _model.config.accel.dev; @@ -182,14 +182,14 @@ int Blackbox::begin() rxConfigMutable()->airModeActivateThreshold = 40; rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; - rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilter.harmonics; - rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilter.q; - rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilter.minFreq; - rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilter.fade; - rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilter.freqLpf; - rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilter.weights[0]; - rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilter.weights[1]; - rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilter.weights[2]; + rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.gyro.rpmFilter.harmonics; + rpmFilterConfigMutable()->rpm_filter_q = _model.config.gyro.rpmFilter.q; + rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.gyro.rpmFilter.minFreq; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.gyro.rpmFilter.fade; + rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.gyro.rpmFilter.freqLpf; + rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.gyro.rpmFilter.weights[0]; + rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.gyro.rpmFilter.weights[1]; + rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.gyro.rpmFilter.weights[2]; updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.modeMaskPresent & 1 << MODE_ARMED); updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.modeMaskPresent & 1 << MODE_ANGLE); diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index f47934f..2df27ca 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -448,18 +448,18 @@ class Cli Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff), Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), - Param(PSTR("gyro_dyn_notch_q"), &c.dynamicFilter.q), - Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width), - Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq), - Param(PSTR("gyro_dyn_notch_max"), &c.dynamicFilter.max_freq), - Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilter.harmonics), - Param(PSTR("gyro_rpm_q"), &c.rpmFilter.q), - Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilter.minFreq), - Param(PSTR("gyro_rpm_fade"), &c.rpmFilter.fade), - Param(PSTR("gyro_rpm_weight_1"), &c.rpmFilter.weights[0]), - Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilter.weights[1]), - Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilter.weights[2]), - Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilter.freqLpf), + Param(PSTR("gyro_dyn_notch_q"), &c.gyro.dynamicFilter.q), + Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.width), + Param(PSTR("gyro_dyn_notch_min"), &c.gyro.dynamicFilter.min_freq), + Param(PSTR("gyro_dyn_notch_max"), &c.gyro.dynamicFilter.max_freq), + Param(PSTR("gyro_rpm_harmonics"), &c.gyro.rpmFilter.harmonics), + Param(PSTR("gyro_rpm_q"), &c.gyro.rpmFilter.q), + Param(PSTR("gyro_rpm_min_freq"), &c.gyro.rpmFilter.minFreq), + Param(PSTR("gyro_rpm_fade"), &c.gyro.rpmFilter.fade), + Param(PSTR("gyro_rpm_weight_1"), &c.gyro.rpmFilter.weights[0]), + Param(PSTR("gyro_rpm_weight_2"), &c.gyro.rpmFilter.weights[1]), + Param(PSTR("gyro_rpm_weight_3"), &c.gyro.rpmFilter.weights[2]), + Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.gyro.rpmFilter.freqLpf), Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]), Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index d146706..3a21d10 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -416,9 +416,9 @@ class Model 1 << (BUZZER_ARMING - 1) | 1 << (BUZZER_BAT_LOW - 1); - if(config.dynamicFilter.width > 6) + if(config.gyro.dynamicFilter.width > 6) { - config.dynamicFilter.width = 6; + config.gyro.dynamicFilter.width = 6; } } @@ -456,7 +456,7 @@ class Model { if(isActive(FEATURE_DYNAMIC_FILTER)) { - for(size_t p = 0; p < (size_t)config.dynamicFilter.width; p++) + for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.width; p++) { state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } @@ -477,10 +477,10 @@ class Model state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilter.freqLpf), gyroFilterRate); - for(size_t n = 0; n < config.rpmFilter.harmonics; n++) + state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); + for(size_t n = 0; n < config.gyro.rpmFilter.harmonics; n++) { - int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilter.harmonics, config.rpmFilter.minFreq, gyroFilterRate / 2); + int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 9b071e1..4e54bc9 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -564,21 +564,23 @@ struct GyroConfig int8_t dev = GYRO_AUTO; int8_t dlpf = GYRO_DLPF_256; int8_t align = ALIGN_DEFAULT; + int16_t bias[3] = { 0, 0, 0 }; FilterConfig filter{FILTER_PT1, 100}; FilterConfig filter2{FILTER_PT1, 213}; FilterConfig filter3{FILTER_FO, 150}; FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; - int16_t bias[3] = { 0, 0, 0 }; + DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; + RpmFilterConfig rpmFilter; }; struct AccelConfig { int8_t bus = BUS_AUTO; int8_t dev = GYRO_AUTO; - FilterConfig filter{FILTER_BIQUAD, 15}; int16_t bias[3] = { 0, 0, 0 }; + FilterConfig filter{FILTER_BIQUAD, 15}; }; struct BaroConfig @@ -593,9 +595,9 @@ struct MagConfig int8_t bus = BUS_AUTO; int8_t dev = MAG_NONE; int8_t align = ALIGN_DEFAULT; - FilterConfig filter{FILTER_BIQUAD, 10}; int16_t scale[3] = { 0, 0, 0 }; int16_t offset[3] = { 1000, 1000, 1000 }; + FilterConfig filter{FILTER_BIQUAD, 10}; }; struct YawConfig @@ -694,6 +696,7 @@ class ModelConfig int8_t pin[PIN_COUNT]; int16_t i2cSpeed = 800; + int8_t tpaScale = 10; int16_t tpaBreakpoint = 1650; @@ -702,10 +705,6 @@ class ModelConfig WirelessConfig wireless; - DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; - - RpmFilterConfig rpmFilter; - uint8_t rescueConfigDelay = 30; int16_t boardAlignment[3] = {0, 0, 0}; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index e4d15bf..89e61ae 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1085,14 +1085,14 @@ class MspProcessor r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range - r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent - r.writeU16(_model.config.dynamicFilter.q); // dyn_notch_q - r.writeU16(_model.config.dynamicFilter.min_freq); // dyn_notch_min_hz + r.writeU8(_model.config.gyro.dynamicFilter.width); // dyn_notch_width_percent + r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q + r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz // rpm filter - r.writeU8(_model.config.rpmFilter.harmonics); // gyro_rpm_notch_harmonics - r.writeU8(_model.config.rpmFilter.minFreq); // gyro_rpm_notch_min + r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min // 1.43+ - r.writeU16(_model.config.dynamicFilter.max_freq); // dyn_notch_max_hz + r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz break; case MSP_SET_FILTER_CONFIG: @@ -1131,15 +1131,15 @@ class MspProcessor } if (m.remain() >= 8) { m.readU8(); // deprecated dyn_notch_range - _model.config.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent - _model.config.dynamicFilter.q = m.readU16(); // dyn_notch_q - _model.config.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - _model.config.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics - _model.config.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min + _model.config.gyro.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent + _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q + _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz + _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min } // 1.43+ if (m.remain() >= 1) { - _model.config.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz + _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz } _model.reload(); break; diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 6583bb5..92753d7 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -30,26 +30,26 @@ int GyroSensor::begin() _sma.begin(_model.config.loopSync); _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); - _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.gyro.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; - _rpm_enabled = _model.config.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; + _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; _rpm_motor_index = 0; - _rpm_fade_inv = 1.0f / _model.config.rpmFilter.fade; - _rpm_min_freq = _model.config.rpmFilter.minFreq; + _rpm_fade_inv = 1.0f / _model.config.gyro.rpmFilter.fade; + _rpm_min_freq = _model.config.gyro.rpmFilter.minFreq; _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; - _rpm_q = _model.config.rpmFilter.q * 0.01f; + _rpm_q = _model.config.gyro.rpmFilter.q * 0.01f; for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { - _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilter.weights[i], 0.0f, 1.0f); + _rpm_weights[i] = Math::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); } for (size_t i = 0; i < 3; i++) { #ifdef ESPFC_DSP - _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter, i); + _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter, i); #else - _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter); + _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter); #endif } @@ -111,7 +111,7 @@ int FAST_CODE_ATTR GyroSensor::filter() { for (size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - for (size_t n = 0; n < _model.config.rpmFilter.harmonics; n++) + for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); } @@ -133,7 +133,7 @@ int FAST_CODE_ATTR GyroSensor::filter() if (_dyn_notch_enabled) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); } @@ -165,12 +165,12 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); const float motorFreq = _model.state.outputTelemetryFreq[_rpm_motor_index]; - for (size_t n = 0; n < _model.config.rpmFilter.harmonics; n++) + for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); const float freqMargin = freq - _rpm_min_freq; float weight = _rpm_weights[n]; - if (freqMargin < _model.config.rpmFilter.fade) + if (freqMargin < _model.config.gyro.rpmFilter.fade) { weight *= freqMargin * _rpm_fade_inv; } @@ -200,7 +200,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() { Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); - const float q = _model.config.dynamicFilter.q * 0.01; + const float q = _model.config.gyro.dynamicFilter.q * 0.01; bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; bool update = _model.state.dynamicFilterTimer.check(); @@ -227,10 +227,10 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() } if (_dyn_notch_enabled && status) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { float freq = _fft[i].peaks[p].freq; - if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); } @@ -258,13 +258,13 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() } if (_dyn_notch_enabled && update) { - if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; - int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.dynamicFilter.min_freq, _model.config.dynamicFilter.max_freq); + int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq); _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); } }