From fada2d20af4d8acfbdc879604420aa703b2252b8 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 20:09:35 +0100 Subject: [PATCH] gyro config refactor --- lib/Espfc/src/Actuator.h | 4 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 22 ++++----- lib/Espfc/src/Cli.h | 44 ++++++++--------- lib/Espfc/src/Hardware.h | 4 +- lib/Espfc/src/Model.h | 20 ++++---- lib/Espfc/src/ModelConfig.h | 30 +++++++----- lib/Espfc/src/Msp/MspProcessor.h | 74 ++++++++++++++--------------- lib/Espfc/src/Sensor/AccelSensor.h | 2 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 8 ++-- test/test_fc/test_fc.cpp | 12 ++--- 10 files changed, 113 insertions(+), 107 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 83d77af..147bd76 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -221,8 +221,8 @@ class Actuator { return; // temporary disable int scale = Math::clamp((int)_model.state.inputUs[AXIS_THRUST], 1000, 2000); - if(_model.config.gyroDynLpfFilter.cutoff > 0) { - int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyroDynLpfFilter.cutoff, _model.config.gyroDynLpfFilter.freq); + if(_model.config.gyro.dynLpfFilter.cutoff > 0) { + int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); for(size_t i = 0; i <= AXIS_YAW; i++) { _model.state.gyroFilter[i].reconfigure(gyroFreq); } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index bb179c1..e31340c 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -110,18 +110,18 @@ int Blackbox::begin() rcControlsConfigMutable()->deadband = _model.config.input.deadband; rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband; - gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyroDlpf; - gyroConfigMutable()->gyro_lpf1_type = _model.config.gyroFilter.type; - gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyroFilter.freq; - gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyroDynLpfFilter.cutoff; - gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyroDynLpfFilter.freq; + gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyro.dlpf; + gyroConfigMutable()->gyro_lpf1_type = _model.config.gyro.filter.type; + gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyro.filter.freq; + gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyro.dynLpfFilter.cutoff; + gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyro.dynLpfFilter.freq; gyroConfigMutable()->gyro_lpf1_dyn_expo = 5; - gyroConfigMutable()->gyro_lpf2_type = _model.config.gyroFilter2.type; - gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyroFilter2.freq; - gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyroNotch1Filter.cutoff; - gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq; - gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff; - gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq; + gyroConfigMutable()->gyro_lpf2_type = _model.config.gyro.filter2.type; + gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyro.filter2.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyro.notch1Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyro.notch1Filter.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyro.notch2Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.freq; gyroConfigMutable()->gyro_sync_denom = 1; dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 27bdbfc..29e5907 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -432,22 +432,22 @@ class Cli Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), Param(PSTR("debug_axis"), &c.debugAxis), - Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices), - Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices), - Param(PSTR("gyro_dlpf"), &c.gyroDlpf, gyroDlpfChoices), - Param(PSTR("gyro_align"), &c.gyroAlign, alignChoices), - Param(PSTR("gyro_lpf_type"), &c.gyroFilter.type, filterTypeChoices), - Param(PSTR("gyro_lpf_freq"), &c.gyroFilter.freq), - Param(PSTR("gyro_lpf2_type"), &c.gyroFilter2.type, filterTypeChoices), - Param(PSTR("gyro_lpf2_freq"), &c.gyroFilter2.freq), - Param(PSTR("gyro_lpf3_type"), &c.gyroFilter3.type, filterTypeChoices), - Param(PSTR("gyro_lpf3_freq"), &c.gyroFilter3.freq), - Param(PSTR("gyro_notch1_freq"), &c.gyroNotch1Filter.freq), - Param(PSTR("gyro_notch1_cutoff"), &c.gyroNotch1Filter.cutoff), - Param(PSTR("gyro_notch2_freq"), &c.gyroNotch2Filter.freq), - Param(PSTR("gyro_notch2_cutoff"), &c.gyroNotch2Filter.cutoff), - Param(PSTR("gyro_dyn_lpf_min"), &c.gyroDynLpfFilter.cutoff), - Param(PSTR("gyro_dyn_lpf_max"), &c.gyroDynLpfFilter.freq), + Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), + Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), + Param(PSTR("gyro_dlpf"), &c.gyro.dlpf, gyroDlpfChoices), + Param(PSTR("gyro_align"), &c.gyro.align, alignChoices), + Param(PSTR("gyro_lpf_type"), &c.gyro.filter.type, filterTypeChoices), + Param(PSTR("gyro_lpf_freq"), &c.gyro.filter.freq), + Param(PSTR("gyro_lpf2_type"), &c.gyro.filter2.type, filterTypeChoices), + Param(PSTR("gyro_lpf2_freq"), &c.gyro.filter2.freq), + Param(PSTR("gyro_lpf3_type"), &c.gyro.filter3.type, filterTypeChoices), + Param(PSTR("gyro_lpf3_freq"), &c.gyro.filter3.freq), + Param(PSTR("gyro_notch1_freq"), &c.gyro.notch1Filter.freq), + Param(PSTR("gyro_notch1_cutoff"), &c.gyro.notch1Filter.cutoff), + Param(PSTR("gyro_notch2_freq"), &c.gyro.notch2Filter.freq), + 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), @@ -460,9 +460,9 @@ class Cli 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_offset_x"), &c.gyroBias[0]), - Param(PSTR("gyro_offset_y"), &c.gyroBias[1]), - Param(PSTR("gyro_offset_z"), &c.gyroBias[2]), + 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]), Param(PSTR("accel_bus"), &c.accelBus, busDevChoices), Param(PSTR("accel_dev"), &c.accelDev, gyroDevChoices), @@ -1037,9 +1037,9 @@ class Cli if(!cmd.args[1]) { s.print(F(" gyro offset: ")); - s.print(_model.config.gyroBias[0]); s.print(' '); - s.print(_model.config.gyroBias[1]); s.print(' '); - s.print(_model.config.gyroBias[2]); s.print(F(" [")); + s.print(_model.config.gyro.bias[0]); s.print(' '); + s.print(_model.config.gyro.bias[1]); s.print(' '); + s.print(_model.config.gyro.bias[2]); s.print(F(" [")); s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' '); s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' '); s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]")); diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 3a70239..4a41b22 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -96,7 +96,7 @@ class Hardware void detectGyro() { - if(_model.config.gyroDev == GYRO_NONE) return; + if(_model.config.gyro.dev == GYRO_NONE) return; Device::GyroDevice * detectedGyro = nullptr; #if defined(ESPFC_SPI_0) @@ -126,7 +126,7 @@ class Hardware #endif if(!detectedGyro) return; - detectedGyro->setDLPFMode(_model.config.gyroDlpf); + detectedGyro->setDLPFMode(_model.config.gyro.dlpf); _model.state.gyroDev = detectedGyro; _model.state.gyroPresent = (bool)detectedGyro; _model.state.accelPresent = _model.state.gyroPresent && _model.config.accelDev != GYRO_NONE; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index c41df05..b8ffadc 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -109,7 +109,7 @@ class Model bool gyroActive() const /* IRAM_ATTR */ { - return state.gyroPresent && config.gyroDev != GYRO_NONE; + return state.gyroPresent && config.gyro.dev != GYRO_NONE; } bool accelActive() const @@ -461,18 +461,18 @@ class Model state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } } - state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate); - state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, gyroFilterRate); - if(config.gyroDynLpfFilter.cutoff > 0) + state.gyroNotch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate); + state.gyroNotch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate); + if(config.gyro.dynLpfFilter.cutoff > 0) { - state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), gyroFilterRate); + state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate); } else { - state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate); + state.gyroFilter[i].begin(config.gyro.filter, gyroFilterRate); } - state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); - state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate); + state.gyroFilter2[i].begin(config.gyro.filter2, gyroFilterRate); + state.gyroFilter3[i].begin(config.gyro.filter3, gyroPreFilterRate); state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) @@ -574,7 +574,7 @@ class Model // load current sensor calibration for(size_t i = 0; i <= AXIS_YAW; i++) { - state.gyroBias.set(i, config.gyroBias[i] / 1000.0f); + state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); state.accelBias.set(i, config.accelBias[i] / 1000.0f); state.magCalibrationOffset.set(i, config.magCalibrationOffset[i] / 10.0f); state.magCalibrationScale.set(i, config.magCalibrationScale[i] / 1000.0f); @@ -586,7 +586,7 @@ class Model // store current sensor calibration for(size_t i = 0; i < 3; i++) { - config.gyroBias[i] = lrintf(state.gyroBias[i] * 1000.0f); + config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); config.accelBias[i] = lrintf(state.accelBias[i] * 1000.0f); config.magCalibrationOffset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f); config.magCalibrationScale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 77ad905..23e9c58 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -558,21 +558,28 @@ struct IBatConfig int16_t offset = 0; }; +struct GyroConfig +{ + int8_t bus = BUS_AUTO; + int8_t dev = GYRO_AUTO; + int8_t dlpf = GYRO_DLPF_256; + int8_t align = ALIGN_DEFAULT; + + 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 }; +}; + // persistent data class ModelConfig { public: - int8_t gyroBus = BUS_AUTO; - int8_t gyroDev = GYRO_AUTO; - int8_t gyroDlpf = GYRO_DLPF_256; - int8_t gyroAlign = ALIGN_DEFAULT; - - FilterConfig gyroFilter{FILTER_PT1, 100}; - FilterConfig gyroFilter2{FILTER_PT1, 213}; - FilterConfig gyroFilter3{FILTER_FO, 150}; - FilterConfig gyroNotch1Filter{FILTER_NOTCH, 0, 0}; - FilterConfig gyroNotch2Filter{FILTER_NOTCH, 0, 0}; - FilterConfig gyroDynLpfFilter{FILTER_PT1, 425, 170}; + GyroConfig gyro; int8_t accelBus = BUS_AUTO; int8_t accelDev = GYRO_AUTO; @@ -646,7 +653,6 @@ class ModelConfig FusionConfig fusion; - int16_t gyroBias[3] = { 0, 0, 0 }; int16_t accelBias[3] = { 0, 0, 0 }; int16_t magCalibrationScale[3] = { 0, 0, 0 }; int16_t magCalibrationOffset[3] = { 1000, 1000, 1000 }; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index b1f723d..0a8f284 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -532,13 +532,13 @@ class MspProcessor break; case MSP_SENSOR_ALIGNMENT: - r.writeU8(_model.config.gyroAlign); // gyro align - r.writeU8(_model.config.gyroAlign); // acc align, Starting with 4.0 gyro and acc alignment are the same + r.writeU8(_model.config.gyro.align); // gyro align + r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same r.writeU8(_model.config.magAlign); // mag align //1.41+ r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK r.writeU8(0); // gyro_to_use - r.writeU8(_model.config.gyroAlign); // gyro 1 + r.writeU8(_model.config.gyro.align); // gyro 1 r.writeU8(0); // gyro 2 break; @@ -554,7 +554,7 @@ class MspProcessor gyroAlign = m.readU8(); // gyro 1 align m.readU8(); // gyro 2 align } - _model.config.gyroAlign = gyroAlign; + _model.config.gyro.align = gyroAlign; } break; @@ -1060,29 +1060,29 @@ class MspProcessor // break; case MSP_FILTER_CONFIG: - r.writeU8(_model.config.gyroFilter.freq); // gyro lpf - r.writeU16(_model.config.dtermFilter.freq); // dterm lpf - r.writeU16(_model.config.yawFilter.freq); // yaw lpf - r.writeU16(_model.config.gyroNotch1Filter.freq); // gyro notch 1 hz - r.writeU16(_model.config.gyroNotch1Filter.cutoff); // gyro notch 1 cutoff - r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz - r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff - r.writeU16(_model.config.gyroNotch2Filter.freq); // gyro notch 2 hz - r.writeU16(_model.config.gyroNotch2Filter.cutoff); // gyro notch 2 cutoff - r.writeU8(_model.config.dtermFilter.type); // dterm type - r.writeU8(fromGyroDlpf(_model.config.gyroDlpf)); - r.writeU8(0); // dlfp 32khz type - r.writeU16(_model.config.gyroFilter.freq); // lowpass1 freq - r.writeU16(_model.config.gyroFilter2.freq); // lowpass2 freq - r.writeU8(_model.config.gyroFilter.type); // lowpass1 type - r.writeU8(_model.config.gyroFilter2.type); // lowpass2 type - r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq + r.writeU8(_model.config.gyro.filter.freq); // gyro lpf + r.writeU16(_model.config.dtermFilter.freq); // dterm lpf + r.writeU16(_model.config.yawFilter.freq); // yaw lpf + r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz + r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff + r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff + r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz + r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff + r.writeU8(_model.config.dtermFilter.type); // dterm type + r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); + r.writeU8(0); // dlfp 32khz type + r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq + r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq + r.writeU8(_model.config.gyro.filter.type); // lowpass1 type + r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type + r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq // 1.41+ - r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type - r.writeU16(_model.config.gyroDynLpfFilter.cutoff); // dyn lpf gyro min - r.writeU16(_model.config.gyroDynLpfFilter.freq); // dyn lpf gyro max - r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min - r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max + r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type + r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min + r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max + r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min + r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent @@ -1096,18 +1096,18 @@ class MspProcessor break; case MSP_SET_FILTER_CONFIG: - _model.config.gyroFilter.freq = m.readU8(); + _model.config.gyro.filter.freq = m.readU8(); _model.config.dtermFilter.freq = m.readU16(); _model.config.yawFilter.freq = m.readU16(); if (m.remain() >= 8) { - _model.config.gyroNotch1Filter.freq = m.readU16(); - _model.config.gyroNotch1Filter.cutoff = m.readU16(); + _model.config.gyro.notch1Filter.freq = m.readU16(); + _model.config.gyro.notch1Filter.cutoff = m.readU16(); _model.config.dtermNotchFilter.freq = m.readU16(); _model.config.dtermNotchFilter.cutoff = m.readU16(); } if (m.remain() >= 4) { - _model.config.gyroNotch2Filter.freq = m.readU16(); - _model.config.gyroNotch2Filter.cutoff = m.readU16(); + _model.config.gyro.notch2Filter.freq = m.readU16(); + _model.config.gyro.notch2Filter.cutoff = m.readU16(); } if (m.remain() >= 1) { _model.config.dtermFilter.type = (FilterType)m.readU8(); @@ -1115,17 +1115,17 @@ class MspProcessor if (m.remain() >= 10) { m.readU8(); // dlfp type m.readU8(); // 32k dlfp type - _model.config.gyroFilter.freq = m.readU16(); - _model.config.gyroFilter2.freq = m.readU16(); - _model.config.gyroFilter.type = m.readU8(); - _model.config.gyroFilter2.type = m.readU8(); + _model.config.gyro.filter.freq = m.readU16(); + _model.config.gyro.filter2.freq = m.readU16(); + _model.config.gyro.filter.type = m.readU8(); + _model.config.gyro.filter2.type = m.readU8(); _model.config.dtermFilter2.freq = m.readU16(); } // 1.41+ if (m.remain() >= 9) { _model.config.dtermFilter2.type = m.readU8(); - _model.config.gyroDynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min - _model.config.gyroDynLpfFilter.freq = m.readU16(); // dyn gyro lpf max + _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min + _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max _model.config.dtermDynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min _model.config.dtermDynLpfFilter.freq = m.readU16(); // dyn dterm lpf min } diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 91221e9..8e9d100 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -64,7 +64,7 @@ class AccelSensor: public BaseSensor _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale; - align(_model.state.accel, _model.config.gyroAlign); + align(_model.state.accel, _model.config.gyro.align); _model.state.accel = _model.state.boardAlignment.apply(_model.state.accel); for(size_t i = 0; i < 3; i++) diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 08dc687..6583bb5 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -19,7 +19,7 @@ int GyroSensor::begin() _gyro = _model.state.gyroDev; if (!_gyro) return 0; - _gyro->setDLPFMode(_model.config.gyroDlpf); + _gyro->setDLPFMode(_model.config.gyro.dlpf); _gyro->setRate(_gyro->getRate()); _model.state.gyroScale = Math::toRad(2000.f) / 32768.f; @@ -53,7 +53,7 @@ int GyroSensor::begin() #endif } - _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); + _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyro.dlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); return 1; } @@ -68,10 +68,10 @@ int FAST_CODE_ATTR GyroSensor::read() VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; - align(input, _model.config.gyroAlign); + align(input, _model.config.gyro.align); input = _model.state.boardAlignment.apply(input); - if (_model.config.gyroFilter3.freq) + if (_model.config.gyro.filter3.freq) { _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); } diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index b079710..ca8740c 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -106,7 +106,7 @@ void test_model_gyro_init_1k_256dlpf() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.begin(); @@ -123,7 +123,7 @@ void test_model_gyro_init_1k_188dlpf() { Model model; model.state.gyroClock = 1000; - model.config.gyroDlpf = GYRO_DLPF_188; + model.config.gyro.dlpf = GYRO_DLPF_188; model.config.loopSync = 2; model.config.mixerSync = 2; model.begin(); @@ -140,7 +140,7 @@ void test_model_inner_pid_init() { Model model; model.state.gyroClock = 1000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -172,7 +172,7 @@ void test_model_outer_pid_init() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -196,7 +196,7 @@ void test_controller_rates() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -246,7 +246,7 @@ void test_controller_rates_limit() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX;