From e9386327185e08d05273b0fd73c6d19e13bd1b2e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 21:03:10 +0100 Subject: [PATCH] dterm iterm yaw mixer level config refactor --- lib/Espfc/src/Actuator.h | 4 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 24 +++++----- lib/Espfc/src/Cli.h | 48 +++++++++---------- lib/Espfc/src/Controller.cpp | 18 ++++---- lib/Espfc/src/Model.h | 28 +++++------ lib/Espfc/src/ModelConfig.h | 72 ++++++++++++++++++----------- lib/Espfc/src/Msp/MspProcessor.h | 64 ++++++++++++------------- lib/Espfc/src/Output/Mixer.cpp | 4 +- test/test_fc/test_fc.cpp | 8 ++-- 9 files changed, 144 insertions(+), 126 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 147bd76..f3d22c1 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -227,8 +227,8 @@ class Actuator _model.state.gyroFilter[i].reconfigure(gyroFreq); } } - if(_model.config.dtermDynLpfFilter.cutoff > 0) { - int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dtermDynLpfFilter.cutoff, _model.config.dtermDynLpfFilter.freq); + if(_model.config.dterm.dynLpfFilter.cutoff > 0) { + int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); for(size_t i = 0; i <= AXIS_YAW; i++) { _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 56eeafe..5a91346 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -66,15 +66,15 @@ int Blackbox::begin() } } cp->pidAtMinThrottle = 1; - cp->dterm_lpf1_type = _model.config.dtermFilter.type; - cp->dterm_lpf1_static_hz = _model.config.dtermFilter.freq; - cp->dterm_lpf1_dyn_min_hz = _model.config.dtermDynLpfFilter.cutoff; - cp->dterm_lpf1_dyn_max_hz = _model.config.dtermDynLpfFilter.freq; - cp->dterm_lpf2_type = _model.config.dtermFilter2.type; - cp->dterm_lpf2_static_hz = _model.config.dtermFilter2.freq; - cp->dterm_notch_hz = _model.config.dtermNotchFilter.freq; - cp->dterm_notch_cutoff = _model.config.dtermNotchFilter.cutoff; - cp->yaw_lowpass_hz = _model.config.yawFilter.freq; + cp->dterm_lpf1_type = _model.config.dterm.filter.type; + cp->dterm_lpf1_static_hz = _model.config.dterm.filter.freq; + cp->dterm_lpf1_dyn_min_hz = _model.config.dterm.dynLpfFilter.cutoff; + cp->dterm_lpf1_dyn_max_hz = _model.config.dterm.dynLpfFilter.freq; + cp->dterm_lpf2_type = _model.config.dterm.filter2.type; + cp->dterm_lpf2_static_hz = _model.config.dterm.filter2.freq; + cp->dterm_notch_hz = _model.config.dterm.notchFilter.freq; + cp->dterm_notch_cutoff = _model.config.dterm.notchFilter.cutoff; + cp->yaw_lowpass_hz = _model.config.yaw.filter.freq; cp->itermWindupPointPercent = 80; cp->antiGravityMode = 0; cp->pidSumLimit = 660; @@ -92,14 +92,14 @@ int Blackbox::begin() cp->anti_gravity_cutoff_hz = 100; cp->d_min_gain = 0; cp->d_min_advance = 0; - cp->angle_limit = _model.config.angleLimit; + cp->angle_limit = _model.config.level.angleLimit; cp->angle_earth_ref = 100; cp->horizon_limit_degrees = 135; cp->horizon_delay_ms = 500; cp->thrustLinearization = 0; - cp->iterm_relax = _model.config.itermRelax; + cp->iterm_relax = _model.config.iterm.relax; cp->iterm_relax_type = 1; - cp->iterm_relax_cutoff = _model.config.itermRelaxCutoff; + cp->iterm_relax_cutoff = _model.config.iterm.relaxCutoff; cp->dterm_lpf1_dyn_expo = 5; cp->tpa_low_rate = 20; cp->tpa_low_breakpoint = 1050; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 1c31ab6..f47934f 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -609,34 +609,34 @@ class Cli Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I), Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D), - Param(PSTR("pid_level_angle_limit"), &c.angleLimit), - Param(PSTR("pid_level_rate_limit"), &c.angleRateLimit), - Param(PSTR("pid_level_lpf_type"), &c.levelPtermFilter.type, filterTypeChoices), - Param(PSTR("pid_level_lpf_freq"), &c.levelPtermFilter.freq), - - Param(PSTR("pid_yaw_lpf_type"), &c.yawFilter.type, filterTypeChoices), - Param(PSTR("pid_yaw_lpf_freq"), &c.yawFilter.freq), - - Param(PSTR("pid_dterm_lpf_type"), &c.dtermFilter.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf_freq"), &c.dtermFilter.freq), - Param(PSTR("pid_dterm_lpf2_type"), &c.dtermFilter2.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf2_freq"), &c.dtermFilter2.freq), - Param(PSTR("pid_dterm_notch_freq"), &c.dtermNotchFilter.freq), - Param(PSTR("pid_dterm_notch_cutoff"), &c.dtermNotchFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dtermDynLpfFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dtermDynLpfFilter.freq), - - Param(PSTR("pid_dterm_weight"), &c.dtermSetpointWeight), - Param(PSTR("pid_iterm_limit"), &c.itermLimit), - Param(PSTR("pid_iterm_zero"), &c.lowThrottleZeroIterm), - Param(PSTR("pid_iterm_relax"), &c.itermRelax, inputItermRelaxChoices), - Param(PSTR("pid_iterm_relax_cutoff"), &c.itermRelaxCutoff), + Param(PSTR("pid_level_angle_limit"), &c.level.angleLimit), + Param(PSTR("pid_level_rate_limit"), &c.level.rateLimit), + Param(PSTR("pid_level_lpf_type"), &c.level.ptermFilter.type, filterTypeChoices), + Param(PSTR("pid_level_lpf_freq"), &c.level.ptermFilter.freq), + + Param(PSTR("pid_yaw_lpf_type"), &c.yaw.filter.type, filterTypeChoices), + Param(PSTR("pid_yaw_lpf_freq"), &c.yaw.filter.freq), + + Param(PSTR("pid_dterm_lpf_type"), &c.dterm.filter.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf_freq"), &c.dterm.filter.freq), + Param(PSTR("pid_dterm_lpf2_type"), &c.dterm.filter2.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf2_freq"), &c.dterm.filter2.freq), + Param(PSTR("pid_dterm_notch_freq"), &c.dterm.notchFilter.freq), + Param(PSTR("pid_dterm_notch_cutoff"), &c.dterm.notchFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dterm.dynLpfFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dterm.dynLpfFilter.freq), + + Param(PSTR("pid_dterm_weight"), &c.dterm.setpointWeight), + Param(PSTR("pid_iterm_limit"), &c.iterm.limit), + Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), + Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), + Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), Param(PSTR("pid_tpa_scale"), &c.tpaScale), Param(PSTR("pid_tpa_breakpoint"), &c.tpaBreakpoint), Param(PSTR("mixer_sync"), &c.mixerSync), - Param(PSTR("mixer_type"), &c.mixerType, mixerTypeChoices), - Param(PSTR("mixer_yaw_reverse"), &c.yawReverse), + Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), + Param(PSTR("mixer_yaw_reverse"), &c.mixer.yawReverse), Param(PSTR("mixer_throttle_limit_type"), &c.output.throttleLimitType, throtleLimitTypeChoices), Param(PSTR("mixer_throttle_limit_percent"), &c.output.throttleLimitPercent), Param(PSTR("mixer_output_limit"), &c.output.motorLimit), diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index e3e8101..d44fb63 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -24,7 +24,7 @@ int FAST_CODE_ATTR Controller::update() { Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); resetIterm(); - if(_model.config.mixerType == FC_MIXER_GIMBAL) + if(_model.config.mixer.type == FC_MIXER_GIMBAL) { outerLoopRobot(); } @@ -36,7 +36,7 @@ int FAST_CODE_ATTR Controller::update() { Stats::Measure(_model.state.stats, COUNTER_INNER_PID); - if(_model.config.mixerType == FC_MIXER_GIMBAL) + if(_model.config.mixer.type == FC_MIXER_GIMBAL) { innerLoopRobot(); } @@ -63,14 +63,14 @@ void Controller::outerLoopRobot() if(true || _model.isActive(MODE_ANGLE)) { - angle = _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit); + angle = _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); } else { - angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * radians(_model.config.angleRateLimit); + angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); } _model.state.desiredAngle.set(AXIS_PITCH, angle); - _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * radians(_model.config.angleRateLimit); + _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); if(_model.config.debugMode == DEBUG_ANGLERATE) { @@ -86,7 +86,7 @@ void Controller::innerLoopRobot() //const float angle = acos(v.z); const float angle = std::max(abs(_model.state.angle[AXIS_PITCH]), abs(_model.state.angle[AXIS_ROLL])); - const bool stabilize = angle < radians(_model.config.angleLimit); + const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit); if(stabilize) { _model.state.output[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); @@ -111,8 +111,8 @@ void FAST_CODE_ATTR Controller::outerLoop() if(_model.isActive(MODE_ANGLE)) { _model.state.desiredAngle = VectorFloat( - _model.state.input[AXIS_ROLL] * radians(_model.config.angleLimit), - _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit), + _model.state.input[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), + _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit), _model.state.angle[AXIS_YAW] ); _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.angle[AXIS_ROLL]); @@ -167,7 +167,7 @@ float Controller::getTpaFactor() const void Controller::resetIterm() { if(!_model.isActive(MODE_ARMED) // when not armed - || (!_model.isAirModeActive() && _model.config.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) + || (!_model.isAirModeActive() && _model.config.iterm.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) ) { for(size_t i = 0; i < AXES; i++) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index b83892f..d146706 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -512,7 +512,7 @@ class Model // configure PIDs float pidScale[] = { 1.f, 1.f, 1.f }; - if(config.mixerType == FC_MIXER_GIMBAL) + if(config.mixer.type == FC_MIXER_GIMBAL) { pidScale[AXIS_YAW] = 0.2f; // ROBOT pidScale[AXIS_PITCH] = 20.f; // ROBOT @@ -526,23 +526,23 @@ class Model pid.Ki = (float)pc.I * ITERM_SCALE * pidScale[i]; pid.Kd = (float)pc.D * DTERM_SCALE * pidScale[i]; pid.Kf = (float)pc.F * FTERM_SCALE * pidScale[i]; - pid.iLimit = config.itermLimit * 0.01f; + pid.iLimit = config.iterm.limit * 0.01f; pid.oLimit = 0.66f; pid.rate = state.loopTimer.rate; - pid.dtermNotchFilter.begin(config.dtermNotchFilter, pidFilterRate); - if(config.dtermDynLpfFilter.cutoff > 0) { - pid.dtermFilter.begin(FilterConfig((FilterType)config.dtermFilter.type, config.dtermDynLpfFilter.cutoff), pidFilterRate); + pid.dtermNotchFilter.begin(config.dterm.notchFilter, pidFilterRate); + if(config.dterm.dynLpfFilter.cutoff > 0) { + pid.dtermFilter.begin(FilterConfig((FilterType)config.dterm.filter.type, config.dterm.dynLpfFilter.cutoff), pidFilterRate); } else { - pid.dtermFilter.begin(config.dtermFilter, pidFilterRate); + pid.dtermFilter.begin(config.dterm.filter, pidFilterRate); } - pid.dtermFilter2.begin(config.dtermFilter2, pidFilterRate); + pid.dtermFilter2.begin(config.dterm.filter2, pidFilterRate); pid.ftermFilter.begin(config.input.filterDerivative, pidFilterRate); - pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, config.itermRelaxCutoff), pidFilterRate); + pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, config.iterm.relaxCutoff), pidFilterRate); if(i == AXIS_YAW) { - pid.itermRelax = config.itermRelax == ITERM_RELAX_RPY || config.itermRelax == ITERM_RELAX_RPY_INC ? config.itermRelax : ITERM_RELAX_OFF; - pid.ptermFilter.begin(config.yawFilter, pidFilterRate); + pid.itermRelax = config.iterm.relax == ITERM_RELAX_RPY || config.iterm.relax == ITERM_RELAX_RPY_INC ? config.iterm.relax : ITERM_RELAX_OFF; + pid.ptermFilter.begin(config.yaw.filter, pidFilterRate); } else { - pid.itermRelax = config.itermRelax; + pid.itermRelax = config.iterm.relax; } pid.begin(); } @@ -555,10 +555,10 @@ class Model pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; pid.Kd = (float)pc.D * LEVEL_DTERM_SCALE; pid.Kf = (float)pc.F * LEVEL_FTERM_SCALE; - pid.iLimit = Math::toRad(config.angleRateLimit) * 0.1f; - pid.oLimit = Math::toRad(config.angleRateLimit); + pid.iLimit = Math::toRad(config.level.rateLimit) * 0.1f; + pid.oLimit = Math::toRad(config.level.rateLimit); pid.rate = state.loopTimer.rate; - pid.ptermFilter.begin(config.levelPtermFilter, pidFilterRate); + pid.ptermFilter.begin(config.level.ptermFilter, pidFilterRate); //pid.iLimit = 0.3f; // ROBOT //pid.oLimit = 1.f; // ROBOT pid.begin(); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 21338d2..9b071e1 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -598,6 +598,41 @@ struct MagConfig int16_t offset[3] = { 1000, 1000, 1000 }; }; +struct YawConfig +{ + FilterConfig filter{FILTER_PT1, 90}; +}; + +struct DtermConfig +{ + FilterConfig filter{FILTER_PT1, 128}; + FilterConfig filter2{FILTER_PT1, 128}; + FilterConfig notchFilter{FILTER_NOTCH, 0, 0}; + FilterConfig dynLpfFilter{FILTER_PT1, 145, 60}; + int16_t setpointWeight = 30; +}; + +struct ItermConfig +{ + int8_t limit = 30; + int8_t relax = ITERM_RELAX_RP; + int8_t relaxCutoff = 15; + bool lowThrottleZeroIterm = true; +}; + +struct LevelConfig +{ + FilterConfig ptermFilter{FILTER_PT1, 90}; + int8_t angleLimit = 55; + int16_t rateLimit = 300; +}; + +struct MixerConfiguration +{ + int8_t type = FC_MIXER_QUADX; + bool yawReverse = 0; +}; + // persistent data class ModelConfig { @@ -615,9 +650,6 @@ class ModelConfig OutputConfig output; - int8_t mixerType = FC_MIXER_QUADX; - bool yawReverse = 0; - PidConfig pid[FC_PID_ITEM_COUNT] = { [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH @@ -631,32 +663,18 @@ class ModelConfig [FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // VEL }; - FilterConfig yawFilter{FILTER_PT1, 90}; - - FilterConfig dtermFilter{FILTER_PT1, 128}; - FilterConfig dtermFilter2{FILTER_PT1, 128}; - FilterConfig dtermNotchFilter{FILTER_NOTCH, 0, 0}; - FilterConfig dtermDynLpfFilter{FILTER_PT1, 145, 60}; - FilterConfig levelPtermFilter{FILTER_PT1, 90}; - - int16_t dtermSetpointWeight = 30; - int8_t itermLimit = 30; - int8_t itermRelax = ITERM_RELAX_RP; - int8_t itermRelaxCutoff = 15; - - int8_t angleLimit = 55; - int16_t angleRateLimit = 300; + MixerConfiguration mixer; + YawConfig yaw; + LevelConfig level; + DtermConfig dterm; + ItermConfig iterm; int8_t loopSync = 8; // MPU 1000Hz int8_t mixerSync = 1; int32_t featureMask = ESPFC_FEATURE_MASK; - - bool lowThrottleZeroIterm = true; - bool telemetry = 0; int32_t telemetryInterval = 1000; - int8_t telemetryPort; // unused BlackboxConfig blackbox; @@ -854,7 +872,7 @@ class ModelConfig void brobot() { - mixerType = FC_MIXER_GIMBAL; + mixer.type = FC_MIXER_GIMBAL; pin[PIN_OUTPUT_0] = 14; // D5 // ROBOT pin[PIN_OUTPUT_1] = 12; // D6 // ROBOT @@ -865,10 +883,10 @@ class ModelConfig //fusionMode = FUSION_COMPLEMENTARY; // ROBOT //accelFilter.freq = 30; // ROBOT - lowThrottleZeroIterm = false; // ROBOT - itermLimit = 10; // ROBOT - dtermSetpointWeight = 0; // ROBOT - angleLimit = 10; // deg // ROBOT + iterm.lowThrottleZeroIterm = false; // ROBOT + iterm.limit = 10; // ROBOT + dterm.setpointWeight = 0; // ROBOT + level.angleLimit = 10; // deg // ROBOT output.protocol = ESC_PROTOCOL_PWM; // ROBOT output.rate = 100; // ROBOT diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 3d12f62..e4d15bf 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -509,13 +509,13 @@ class MspProcessor break; case MSP_MIXER_CONFIG: - r.writeU8(_model.config.mixerType); // mixerMode, QUAD_X - r.writeU8(_model.config.yawReverse); // yaw_motors_reversed + r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X + r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed break; case MSP_SET_MIXER_CONFIG: - _model.config.mixerType = m.readU8(); // mixerMode, QUAD_X - _model.config.yawReverse = m.readU8(); // yaw_motors_reversed + _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X + _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed break; case MSP_SENSOR_CONFIG: @@ -1061,28 +1061,28 @@ class MspProcessor case MSP_FILTER_CONFIG: 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.dterm.filter.freq); // dterm lpf + r.writeU16(_model.config.yaw.filter.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.dterm.notchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dterm.notchFilter.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(_model.config.dterm.filter.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 + r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq // 1.41+ - r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type + r.writeU8(_model.config.dterm.filter2.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 + r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min + 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 @@ -1097,20 +1097,20 @@ class MspProcessor case MSP_SET_FILTER_CONFIG: _model.config.gyro.filter.freq = m.readU8(); - _model.config.dtermFilter.freq = m.readU16(); - _model.config.yawFilter.freq = m.readU16(); + _model.config.dterm.filter.freq = m.readU16(); + _model.config.yaw.filter.freq = m.readU16(); if (m.remain() >= 8) { _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(); + _model.config.dterm.notchFilter.freq = m.readU16(); + _model.config.dterm.notchFilter.cutoff = m.readU16(); } if (m.remain() >= 4) { _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(); + _model.config.dterm.filter.type = (FilterType)m.readU8(); } if (m.remain() >= 10) { m.readU8(); // dlfp type @@ -1119,15 +1119,15 @@ class MspProcessor _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(); + _model.config.dterm.filter2.freq = m.readU16(); } // 1.41+ if (m.remain() >= 9) { - _model.config.dtermFilter2.type = m.readU8(); + _model.config.dterm.filter2.type = m.readU8(); _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 + _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min + _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min } if (m.remain() >= 8) { m.readU8(); // deprecated dyn_notch_range @@ -1178,20 +1178,20 @@ class MspProcessor r.writeU8(0); // reserved r.writeU8(0); // vbatPidCompensation; r.writeU8(0); // feedForwardTransition; - r.writeU8((uint8_t)std::min(_model.config.dtermSetpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight + r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight r.writeU8(0); // reserved r.writeU8(0); // reserved r.writeU8(0); // reserved r.writeU16(0); // rateAccelLimit; r.writeU16(0); // yawRateAccelLimit; - r.writeU8(_model.config.angleLimit); // levelAngleLimit; + r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; r.writeU8(0); // was pidProfile.levelSensitivity r.writeU16(0); // itermThrottleThreshold; r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ - r.writeU16(_model.config.dtermSetpointWeight); + r.writeU16(_model.config.dterm.setpointWeight); r.writeU8(0); // iterm rotation r.writeU8(0); // smart feed forward - r.writeU8(_model.config.itermRelax); // iterm relax + r.writeU8(_model.config.iterm.relax); // iterm relax r.writeU8(1); // iterm relax type (setpoint only) r.writeU8(0); // abs control gain r.writeU8(0); // throttle boost @@ -1209,7 +1209,7 @@ class MspProcessor r.writeU8(0); // use_integrated_yaw r.writeU8(0); // integrated_yaw_relax // 1.42+ - r.writeU8(_model.config.itermRelaxCutoff); // iterm_relax_cutoff + r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff // 1.43+ r.writeU8(_model.config.output.motorLimit); // motor_output_limit r.writeU8(0); // auto_profile_cell_count @@ -1223,14 +1223,14 @@ class MspProcessor m.readU8(); // reserved m.readU8(); m.readU8(); - _model.config.dtermSetpointWeight = m.readU8(); + _model.config.dterm.setpointWeight = m.readU8(); m.readU8(); // reserved m.readU8(); // reserved m.readU8(); // reserved m.readU16(); m.readU16(); if (m.remain() >= 2) { - _model.config.angleLimit = m.readU8(); + _model.config.level.angleLimit = m.readU8(); m.readU8(); // was pidProfile.levelSensitivity } if (m.remain() >= 4) { @@ -1238,12 +1238,12 @@ class MspProcessor m.readU16(); // itermAcceleratorGain; anti_gravity_gain } if (m.remain() >= 2) { - _model.config.dtermSetpointWeight = m.readU16(); + _model.config.dterm.setpointWeight = m.readU16(); } if (m.remain() >= 14) { m.readU8(); //iterm rotation m.readU8(); //smart feed forward - _model.config.itermRelax = m.readU8(); //iterm relax + _model.config.iterm.relax = m.readU8(); //iterm relax m.readU8(); //iterm relax type m.readU8(); //abs control gain m.readU8(); //throttle boost @@ -1265,7 +1265,7 @@ class MspProcessor } // 1.42+ if (m.remain() >= 1) { - _model.config.itermRelaxCutoff = m.readU8(); // iterm_relax_cutoff + _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff } // 1.43+ if (m.remain() >= 3) { diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 988a01b..eccbed0 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -75,7 +75,7 @@ int Mixer::begin() _model.state.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; _model.state.maxThrottle = 2000.f; } - _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixerType, _model.state.customMixer); + _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixer.type, _model.state.customMixer); return 1; } @@ -115,7 +115,7 @@ void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs sources[MIXER_SOURCE_ROLL] = _model.state.output[AXIS_ROLL]; sources[MIXER_SOURCE_PITCH] = _model.state.output[AXIS_PITCH]; - sources[MIXER_SOURCE_YAW] = _model.state.output[AXIS_YAW] * (_model.config.yawReverse ? 1.f : -1.f); + sources[MIXER_SOURCE_YAW] = _model.state.output[AXIS_YAW] * (_model.config.mixer.yawReverse ? 1.f : -1.f); sources[MIXER_SOURCE_THRUST] = _model.state.output[AXIS_THRUST]; sources[MIXER_SOURCE_RC_ROLL] = _model.state.input[AXIS_ROLL]; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index ca8740c..538b5f2 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -143,7 +143,7 @@ void test_model_inner_pid_init() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.pid[FC_PID_ROLL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.config.pid[FC_PID_PITCH] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.config.pid[FC_PID_YAW] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; @@ -175,7 +175,7 @@ void test_model_outer_pid_init() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.pid[FC_PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.begin(); @@ -199,7 +199,7 @@ void test_controller_rates() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70; @@ -249,7 +249,7 @@ void test_controller_rates_limit() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70;