EmuBoost2.0 (should perhaps have a different name)

src/main/blackbox/blackbox.c

@@ -1218,6 +1218,11 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d",                      currentPidProfile->mixer_impl);
         BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d",                  currentPidProfile->mixer_laziness);
         BLACKBOX_PRINT_HEADER_LINE("mixer_yaw_throttle_comp", "%d",         currentPidProfile->mixer_yaw_throttle_comp);
+        BLACKBOX_PRINT_HEADER_LINE("emuboost2", "%d",                       currentPidProfile->emuBoost2);
+        BLACKBOX_PRINT_HEADER_LINE("emuboost2_filter", "%d",                currentPidProfile->emuBoost2_filter);
+        BLACKBOX_PRINT_HEADER_LINE("emuboost2_cutoff", "%d",                currentPidProfile->emuBoost2_cutoff);
+        BLACKBOX_PRINT_HEADER_LINE("emuboost2_expo", "%d",                  currentPidProfile->emuBoost2_expo);
+
         // End of EmuFlight controller parameters
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);
@@ -1249,10 +1254,11 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_alpha", "%d",                  gyroConfig()->gyro_ABG_alpha);
         BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_boost", "%d",                  gyroConfig()->gyro_ABG_boost);
         BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_half_life", "%d",              gyroConfig()->gyro_ABG_half_life);
-        BLACKBOX_PRINT_HEADER_LINE("smith_predict_enabled", "%d",              gyroConfig()->smithPredictorEnabled);
+        BLACKBOX_PRINT_HEADER_LINE("smith_predict_enabled", "%d",           gyroConfig()->smithPredictorEnabled);
         BLACKBOX_PRINT_HEADER_LINE("smith_predict_str", "%d",               gyroConfig()->smithPredictorStrength);
         BLACKBOX_PRINT_HEADER_LINE("smith_predict_delay", "%d",             gyroConfig()->smithPredictorDelay);
         BLACKBOX_PRINT_HEADER_LINE("smith_predict_filt_hz", "%d",           gyroConfig()->smithPredictorFilterHz);
+
 #if defined(USE_ACC)
         BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d",                  (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
         BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d",                accelerometerConfig()->acc_hardware);

src/main/build/debug.c

@@ -77,5 +77,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "IMU",
     "KALMAN",
     "ANGLE",
-    "HORIZON"
+    "HORIZON",
+    "EMUBOOST"
 };

src/main/build/debug.h

@@ -96,6 +96,7 @@ typedef enum {
     DEBUG_KALMAN,
     DEBUG_ANGLE,
     DEBUG_HORIZON,
+    DEBUG_EMUBOOST,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cms/cms_menu_imu.c

@@ -83,6 +83,11 @@ static uint8_t linear_throttle;
 static mixerImplType_e mixer_impl;
 static uint8_t mixer_laziness;
 static uint8_t mixer_yaw_throttle_comp;
+static uint8_t emuBoost2;
+static uint8_t emuBoost2_filter;
+static uint8_t emuBoost2_cutoff;
+static uint8_t emuBoost2_expo;
+
 static uint8_t tmpRateProfileIndex;
 static uint8_t rateProfileIndex;
 static char rateProfileIndexString[] = " p-r";
@@ -164,6 +169,10 @@ static long cmsx_PidAdvancedRead(void) {
     mixer_impl = pidProfile->mixer_impl;
     mixer_laziness = pidProfile->mixer_laziness;
     mixer_yaw_throttle_comp = pidProfile->mixer_yaw_throttle_comp;
+    emuBoost2 = pidProfile->emuBoost2;
+    emuBoost2_filter = pidProfile->emuBoost2_filter;
+    emuBoost2_cutoff = pidProfile->emuBoost2_cutoff;
+    emuBoost2_expo = pidProfile->emuBoost2_expo;
     return 0;
 }
 
@@ -197,6 +206,10 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) {
     pidProfile->mixer_impl = mixer_impl;
     pidProfile->mixer_laziness = mixer_laziness;
     pidProfile->mixer_yaw_throttle_comp = mixer_yaw_throttle_comp;
+    pidProfile->emuBoost2 = emuBoost2;
+    pidProfile->emuBoost2_filter = emuBoost2_filter;
+    pidProfile->emuBoost2_cutoff = emuBoost2_cutoff;
+    pidProfile->emuBoost2_expo = emuBoost2_expo;
     pidInitConfig(currentPidProfile);
     return 0;
 }
@@ -231,6 +244,11 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
     { "MIXER LAZINESS",    OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 },
     { "MIXER YAW THR COMP", OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &mixer_yaw_throttle_comp, 1, cms_offOnLabels }, 0 },
 
+    { "EMUBOOST 2.0",      OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2,       0,  250,  1}, 0 },
+    { "EMUBOOST 2.0 FILT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_filter,       1,  250,  1}, 0 },
+    { "EMUBOOST 2.0 CUT",  OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_cutoff,       5,  250,  1}, 0 },
+    { "EMUBOOST 2.0 EXPO",  OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_expo,       10,  100,  1}, 0 },
+
     { "SAVE&EXIT",         OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVE, 0},
     { "BACK",              OME_Back, NULL, NULL, 0 },
     { NULL,                OME_END, NULL, NULL, 0 }

src/main/common/filter.c

@@ -221,14 +221,14 @@ FAST_CODE float alphaBetaGammaApply(alphaBetaGammaFilter_t *filter, float input)
   // update (estimated) velocity
   filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
   filter->ak += filter->dT * filter->jk;
-  
+
   // what is our residual error (measured - estimated)
   rk = input - filter->xk;
-  
+
   // artificially boost the error to increase the response of the filter
   rk += pt1FilterApply(&filter->boostFilter, (fabsf(rk) * rk * filter->boost));
   filter->rk = rk; // for logging
-  
+
   // update our estimates given the residual error.
   filter->xk += filter->a * rk;
   filter->vk += filter->b / filter->dT * rk;

src/main/flight/pid.c

@@ -197,6 +197,10 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .angle_filter = 100,
     .dtermDynNotch = false,
     .dterm_dyn_notch_q = 400,
+    .emuBoost2 = 150,
+    .emuBoost2_filter = 50,
+    .emuBoost2_cutoff = 5,
+    .emuBoost2_expo = 35,
                 );
 }
 
@@ -246,6 +250,8 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffYaw;
 static FAST_RAM_ZERO_INIT pt1Filter_t emuGravityThrottleLpf;
 static FAST_RAM_ZERO_INIT pt1Filter_t axisLockLpf[XYZ_AXIS_COUNT];
 
+static FAST_RAM_ZERO_INIT pt1Filter_t emuboost2_0Filter[XYZ_AXIS_COUNT];
+
 static FAST_RAM_ZERO_INIT float iDecay;
 
 void pidInitFilters(const pidProfile_t *pidProfile) {
@@ -333,6 +339,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&axisLockLpf[i], pt1FilterGain(pidProfile->axis_lock_hz, dT));
+        pt1FilterInit(&emuboost2_0Filter[i], pt1FilterGain(pidProfile->emuBoost2_filter, dT));
 #if defined(USE_ITERM_RELAX)
         if (i != FD_YAW) {
             pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
@@ -674,13 +681,12 @@ static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
 #ifdef USE_GYRO_DATA_ANALYSE
 static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5];
 #endif
+static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float dynamicDtermScaler[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
 
 void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
-    float axisLock[XYZ_AXIS_COUNT];
-    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-        axisLock[axis] = pt1FilterApply(&axisLockLpf[axis], stickMovement[axis]) * axisLockMultiplier;
-    }
 
     scaledAxisPid[ROLL] = constrainf(1 - axisLock[PITCH] - axisLock[YAW] + axisLock[ROLL], 0.0f, 1.0f);
     scaledAxisPid[PITCH] = constrainf(1 - axisLock[ROLL] - axisLock[YAW] + axisLock[PITCH], 0.0f, 1.0f);
@@ -695,6 +701,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
+        axisLock[axis] = pt1FilterApply(&axisLockLpf[axis], stickMovement[axis]) * axisLockMultiplier;
+
         // emugravity, the different hopefully better version of antiGravity no effect on yaw
         float errorAccelerator = 1.0f;
         if (pidProfile->emuGravityGain != 0 && axis != FD_YAW) {
@@ -768,9 +776,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
             const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
             if (axis != FD_YAW) {
-                itermRelaxFactor = MAX(1 - setpointHpf / pidProfile->iterm_relax_threshold, 0.0f);
+                itermRelaxFactor = MAX(1.0f - setpointHpf / pidProfile->iterm_relax_threshold, 0.0f);
             } else {
-                itermRelaxFactor = MAX(1 - setpointHpf / pidProfile->iterm_relax_threshold_yaw, 0.0f);
+                itermRelaxFactor = MAX(1.0f - setpointHpf / pidProfile->iterm_relax_threshold_yaw, 0.0f);
             }
             if (SIGN(iterm) == SIGN(itermErrorRate)) {
                 itermErrorRate *= itermRelaxFactor;
@@ -804,14 +812,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             temporaryIterm[axis] = iterm;
         }
         // -----calculate D component
+        float pureError = 0.0f;
+        //filter Kd properly, no setpoint filtering
+        pureError = errorRate - previousError[axis];
+        previousError[axis] = errorRate;
+
+        float dtermErrorRate = currentPidSetpoint - gyroRate * dynamicDtermScaler[axis]; // r - y
+        float dtermError = dtermErrorRate - previousDtermErrorRate[axis];
+        previousDtermErrorRate[axis] = dtermErrorRate;
+
+        const float pureMeasurement = -(gyroRate - previousMeasurement[axis]);
+        previousMeasurement[axis] = gyroRate;
+
         if (pidCoefficient[axis].Kd > 0) {
-            //filter Kd properly, no setpoint filtering
-            const float pureError = errorRate - previousError[axis];
-            const float pureMeasurement = -(gyroRate - previousMeasurement[axis]);
-            previousMeasurement[axis] = gyroRate;
-            previousError[axis] = errorRate;
-            float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
-            //filter the dterm
+            float dDelta = ((feathered_pids * pureMeasurement) * dynamicDtermScaler[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
+            // filter the dterm
 #ifdef USE_GYRO_DATA_ANALYSE
             if (isDynamicFilterActive() && pidProfile->dtermDynNotch && axis <= gyroConfig()->dyn_notch_axis+1) {
                 for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
@@ -826,7 +841,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta);
             dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta);
             dDelta = dtermABGapplyFn((filter_t *)&dtermABG[axis], dDelta);
-            //dterm boost, similar to emuboost
+            // dterm boost, similar to emuboost
             float boostedDtermRate;
             boostedDtermRate = (dDelta * fabsf(dDelta)) * dtermBoostMultiplier;
             if (fabsf(dDelta * dtermBoostLimitPercent) < fabsf(boostedDtermRate)) {
@@ -888,6 +903,38 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             pidData[axis].D *= getThrottleDAttenuation();
         }
 
+        // emuboost 2.0
+        if (pidProfile->emuBoost2) {
+          float changeError = pureError * pidFrequency * DTERM_SCALE * 20.0f;
+          changeError = changeError / pidProfile->emuBoost2_cutoff;
+
+          float doSignMatch = 0;
+          changeError = constrainf(changeError, -1.0f, 1.0f);
+          changeError = ABS(changeError) * power3(changeError) * (pidProfile->emuBoost2_expo / 100.0f) + changeError * (1 - pidProfile->emuBoost2_expo / 100.0f);
+
+          float scaledError = constrainf(errorRate / pidProfile->emuBoost2_cutoff, -1.0f, 1.0f);
+          scaledError = ABS(scaledError) * power3(scaledError) * (pidProfile->emuBoost2_expo / 100.0f) + scaledError * (1 - pidProfile->emuBoost2_expo / 100.0f);
+
+          scaledError *= changeError; // this is only 1 when the error and change in setpoint are high
+          scaledError = pt1FilterApply(&emuboost2_0Filter[axis], scaledError);
+          if (axis == FD_ROLL || axis == FD_PITCH) {
+              DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
+          }
+          dynamicDtermScaler[axis] = 1 - scaledError; // this is only 0 when the error and change in setpoint are high
+
+          if (scaledError > 0.0f) {
+              pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
+              doSignMatch = 1000.0f;
+          } else {
+              doSignMatch = 0.0f;
+          }
+          if (axis == FD_ROLL || axis == FD_PITCH) {
+            DEBUG_SET(DEBUG_EMUBOOST, axis + 2, lrintf(doSignMatch));
+          }
+        } else {
+            dynamicDtermScaler[axis] = 1.0f;
+        }
+
         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + directFeedForward;
         pidData[axis].Sum = pidSum * scaledAxisPid[axis];
     }
@@ -900,3 +947,7 @@ bool crashRecoveryModeActive(void) {
 float pidGetPreviousSetpoint(int axis) {
     return previousPidSetpoint[axis];
 }
+
+float getDtermPercentLeft(int axis) {
+    return dynamicDtermScaler[axis];
+}

src/main/flight/pid.h

@@ -157,6 +157,10 @@ typedef struct pidProfile_s {
     uint8_t dterm_ABG_half_life;
     uint8_t dtermDynNotch;
     uint16_t dterm_dyn_notch_q;
+    uint8_t emuBoost2;
+    uint8_t emuBoost2_filter;
+    uint8_t emuBoost2_cutoff;
+    uint8_t emuBoost2_expo;
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE
@@ -201,3 +205,4 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 bool crashRecoveryModeActive(void);
 float pidGetPreviousSetpoint(int axis);
 void pidUpdateEmuGravityThrottleFilter(float throttle);
+float getDtermPercentLeft(int axis);

src/main/interface/settings.c

@@ -876,6 +876,10 @@ const clivalue_t valueTable[] = {
     { "dterm_dyn_notch_enable",     VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermDynNotch) },
     { "dterm_dyn_notch_q",          VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) },
 #endif
+    { "emuboost2",                  VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2) },
+    { "emuboost2_filter",           VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_filter) },
+    { "emuboost2_cutoff",           VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 5, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
+    { "emuboost2_expo",             VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_expo) },
     { "dterm_abg_alpha",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },
     { "dterm_abg_boost",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) },
     { "dterm_abg_half_life",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_half_life) },