dterm iterm yaw mixer level config refactor

rtlopez · Oct 27, 2024 · e938632 · e938632
1 parent 2f1b947
commit e938632
Show file tree

Hide file tree

Showing 9 changed files with 144 additions and 126 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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