diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 436871abb4..197b99e706 100644 --- a/src/main/blackbox/blackbox.c +++ b/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); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 84d93a28a6..37419d5826 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -77,5 +77,6 @@ const char * const debugModeNames[DEBUG_COUNT] = { "IMU", "KALMAN", "ANGLE", - "HORIZON" + "HORIZON", + "EMUBOOST" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 8c4a280c11..805d7f7e1d 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -96,6 +96,7 @@ typedef enum { DEBUG_KALMAN, DEBUG_ANGLE, DEBUG_HORIZON, + DEBUG_EMUBOOST, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 50c0bfc589..a5d8c935d7 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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 } diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 6a705a6c80..fbdc018d69 100644 --- a/src/main/common/filter.c +++ b/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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e5e7601ab6..791ecc6243 100644 --- a/src/main/flight/pid.c +++ b/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]; +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ea9f16d811..8a87442bc3 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 3b7d50938c..1c3cc312bc 100644 --- a/src/main/interface/settings.c +++ b/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) },