Skip to content

Commit

Permalink
move rpm and dynamic filter to gyro config
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 27, 2024
1 parent e938632 commit 893c374
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 67 deletions.
24 changes: 12 additions & 12 deletions lib/Espfc/src/Blackbox/Blackbox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
24 changes: 12 additions & 12 deletions lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]),
Expand Down
12 changes: 6 additions & 6 deletions lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}
}
Expand Down
13 changes: 6 additions & 7 deletions lib/Espfc/src/ModelConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -694,6 +696,7 @@ class ModelConfig

int8_t pin[PIN_COUNT];
int16_t i2cSpeed = 800;

int8_t tpaScale = 10;
int16_t tpaBreakpoint = 1650;

Expand All @@ -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};
Expand Down
24 changes: 12 additions & 12 deletions lib/Espfc/src/Msp/MspProcessor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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;
Expand Down
36 changes: 18 additions & 18 deletions lib/Espfc/src/Sensor/GyroSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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();

Expand All @@ -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);
}
Expand Down Expand Up @@ -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);
}
}
Expand Down

0 comments on commit 893c374

Please sign in to comment.