diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9a78eb10e5..bc038b80cd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -476,7 +476,7 @@ static const char * const lookupTableOffOnAuto[] = { }; static const char* const lookupTableInterpolatedSetpoint[] = { - "OFF", "ON", "AVERAGED" + "OFF", "ON", "AVERAGED_2", "AVERAGED_3", "AVERAGED_4" }; static const char* const lookupTableDshotBitbangedTimer[] = { @@ -1097,6 +1097,7 @@ const clivalue_t valueTable[] = { { "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) }, { "ff_spike_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spike_limit) }, { "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) }, + { "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) }, #endif { "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) }, diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c index 050161bf31..308c59873a 100644 --- a/src/main/flight/interpolated_setpoint.c +++ b/src/main/flight/interpolated_setpoint.c @@ -28,15 +28,24 @@ #include "fc/rc.h" #include "flight/interpolated_setpoint.h" +#define PREV_BIG_STEP 1000.0f //threshold for size of jump of packet before the identical data packet + static float setpointDeltaImpl[XYZ_AXIS_COUNT]; -static float prevSetpointDeltaImpl[XYZ_AXIS_COUNT]; static float setpointDelta[XYZ_AXIS_COUNT]; +static uint8_t holdCount[XYZ_AXIS_COUNT]; +typedef struct laggedMovingAverageCombined_u { + laggedMovingAverage_t filter; + float buf[4]; +} laggedMovingAverageCombined_t; +laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT]; + static float prevSetpointSpeed[XYZ_AXIS_COUNT]; +static float prevAcceleration[XYZ_AXIS_COUNT]; static float prevRawSetpoint[XYZ_AXIS_COUNT]; -static float prevSetpointAcceleration[XYZ_AXIS_COUNT]; - +static float prevDeltaImpl[XYZ_AXIS_COUNT]; +static bool bigStep[XYZ_AXIS_COUNT]; // Configuration static float ffMaxRateLimit[XYZ_AXIS_COUNT]; @@ -44,9 +53,11 @@ static float ffMaxRate[XYZ_AXIS_COUNT]; void interpolatedSpInit(const pidProfile_t *pidProfile) { const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f; + uint8_t j = pidProfile->ff_interpolate_sp; for (int i = 0; i < XYZ_AXIS_COUNT; i++) { ffMaxRate[i] = applyCurve(i, 1.0f); ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale; + laggedMovingAverageInit(&setpointDeltaAvg[i].filter, j, (float *)&setpointDeltaAvg[i].buf[0]); } } @@ -58,9 +69,46 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp const float rxInterval = currentRxRefreshRate * 1e-6f; const float rxRate = 1.0f / rxInterval; - const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate; - const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT(); + float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate; + float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; + const uint8_t holdSteps = 2; + // Glitch reduction code for identical packets + if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) { + // identical packets, not at full deflection + if (holdCount[axis] < holdSteps && fabsf(rawSetpoint) > 2.0f && !bigStep[axis]) { + // holding the entire previous speed is best for missed packets, but bad for early packets + setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis]; + setpointAcceleration = prevAcceleration[axis]; + holdCount[axis] += 1; + } else { + // identical packets for more than hold steps, or prev big step + // lock acceleration to zero and don't interpolate forward until sticks move again + holdCount[axis] = holdSteps + 1; + setpointAcceleration = 0.0f; + } + } else { + // we're moving, or sticks are at max + if (holdCount[axis] == 2) { + // we are after an identical packet, and the one before was a normal step up, + // so raw step speed and acceleration of next 'good' packet is twice what it should be + setpointSpeed /= 2.0f; + setpointAcceleration /= 2.0f; + } + if (holdCount[axis] == 3) { + // we are starting to move after a gap after a big step up, or persistent flat period, so accelerate gently + setpointAcceleration = 0.0f; + } + holdCount[axis] = 1; + if (fabsf(setpointAcceleration - prevAcceleration[axis]) > PREV_BIG_STEP) { + bigStep[axis] = true; + } else { + bigStep[axis] = false; + } + } + + prevAcceleration[axis] = setpointAcceleration; + setpointAcceleration *= pidGetDT(); setpointDeltaImpl[axis] = setpointSpeed * pidGetDT(); const float ffBoostFactor = pidGetFfBoostFactor(); @@ -75,25 +123,34 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) { boostAmount = ffBoostFactor * setpointAcceleration; } + // no clip for first step inwards from max deflection + if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) { + clip = 1.0f; + } } + prevSetpointSpeed[axis] = setpointSpeed; - prevSetpointAcceleration[axis] = setpointAcceleration; prevRawSetpoint[axis] = rawSetpoint; + if (axis == FD_ROLL) { DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000); DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000); - DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, boostAmount * clip * 1000); - DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, clip * 100); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, bigStep[axis]); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]); } + setpointDeltaImpl[axis] += boostAmount * clip; + // first order filter FF + const float ffSmoothFactor = pidGetFfSmoothFactor(); + setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]); + prevDeltaImpl[axis] = setpointDeltaImpl[axis]; + if (type == FF_INTERPOLATE_ON) { setpointDelta[axis] = setpointDeltaImpl[axis]; } else { - setpointDelta[axis] = 0.5f * (setpointDeltaImpl[axis] + prevSetpointDeltaImpl[axis]); - prevSetpointDeltaImpl[axis] = setpointDeltaImpl[axis]; + setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]); } } - return setpointDelta[axis]; } diff --git a/src/main/flight/interpolated_setpoint.h b/src/main/flight/interpolated_setpoint.h index 77812a0884..0b26f46c11 100644 --- a/src/main/flight/interpolated_setpoint.h +++ b/src/main/flight/interpolated_setpoint.h @@ -28,7 +28,9 @@ typedef enum ffInterpolationType_e { FF_INTERPOLATE_OFF, FF_INTERPOLATE_ON, - FF_INTERPOLATE_AVG + FF_INTERPOLATE_AVG2, + FF_INTERPOLATE_AVG3, + FF_INTERPOLATE_AVG4 } ffInterpolationType_t; void interpolatedSpInit(const pidProfile_t *pidProfile); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 73c3c31d24..02d5de7abd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -212,9 +212,10 @@ void resetPidProfile(pidProfile_t *pidProfile) .idle_p = 50, .idle_pid_limit = 200, .idle_max_increase = 150, - .ff_interpolate_sp = FF_INTERPOLATE_AVG, - .ff_spike_limit = 60, + .ff_interpolate_sp = FF_INTERPOLATE_AVG2, + .ff_spike_limit = 50, .ff_max_rate_limit = 100, + .ff_smooth_factor = 37, .ff_boost = 15, ); #ifndef USE_D_MIN @@ -314,6 +315,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2; static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf; static FAST_RAM_ZERO_INIT float ffBoostFactor; +static FAST_RAM_ZERO_INIT float ffSmoothFactor; static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse; float pidGetSpikeLimitInverse() @@ -327,6 +329,11 @@ float pidGetFfBoostFactor() return ffBoostFactor; } +float pidGetFfSmoothFactor() +{ + return ffSmoothFactor; +} + void pidInitFilters(const pidProfile_t *pidProfile) { @@ -738,6 +745,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #ifdef USE_INTERPOLATED_SP ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp; + ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f; interpolatedSpInit(pidProfile); #endif } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 691bcb7e02..53f36470c6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -184,6 +184,7 @@ typedef struct pidProfile_s { uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms + uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -261,4 +262,5 @@ float pidGetPreviousSetpoint(int axis); float pidGetDT(); float pidGetPidFrequency(); float pidGetFfBoostFactor(); +float pidGetFfSmoothFactor(); float pidGetSpikeLimitInverse();