From 73663f7fae24b1bfb2fc5f35e6fa9ac246bd02cb Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 12 Sep 2020 17:08:14 +1000 Subject: [PATCH] FF update including second order fitlering of boost, tidying up etc Include default to no averaging --- src/main/blackbox/blackbox.c | 1 - src/main/cli/settings.c | 1 - src/main/flight/interpolated_setpoint.c | 151 +++++++++++------------- src/main/flight/pid.c | 11 +- src/main/flight/pid.h | 3 - src/main/flight/pid_init.c | 1 - 6 files changed, 73 insertions(+), 95 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c53364bc18..cfa333db7a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1390,7 +1390,6 @@ static bool blackboxWriteSysinfo(void) currentPidProfile->pid[PID_YAW].F); #ifdef USE_INTERPOLATED_SP BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d", currentPidProfile->ff_interpolate_sp); - BLACKBOX_PRINT_HEADER_LINE("ff_spike_limit", "%d", currentPidProfile->ff_spike_limit); BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->ff_max_rate_limit); #endif BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->ff_boost); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 00b3cf0748..eb21b01751 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1145,7 +1145,6 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_INTERPOLATED_SP { "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 diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c index 21f77dabd6..296c44f8e6 100644 --- a/src/main/flight/interpolated_setpoint.c +++ b/src/main/flight/interpolated_setpoint.c @@ -35,7 +35,6 @@ static float setpointDeltaImpl[XYZ_AXIS_COUNT]; static float setpointDelta[XYZ_AXIS_COUNT]; -static uint8_t holdCount[XYZ_AXIS_COUNT]; typedef struct laggedMovingAverageCombined_s { laggedMovingAverage_t filter; @@ -47,11 +46,14 @@ laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT]; static float prevSetpointSpeed[XYZ_AXIS_COUNT]; static float prevAcceleration[XYZ_AXIS_COUNT]; static float prevRawSetpoint[XYZ_AXIS_COUNT]; +//for smoothing static float prevDeltaImpl[XYZ_AXIS_COUNT]; +static float prevBoostAmount[XYZ_AXIS_COUNT]; + +static uint8_t ffStatus[XYZ_AXIS_COUNT]; static bool bigStep[XYZ_AXIS_COUNT]; static uint8_t averagingCount; -// Configuration static float ffMaxRateLimit[XYZ_AXIS_COUNT]; static float ffMaxRate[XYZ_AXIS_COUNT]; @@ -69,101 +71,85 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp if (newRcFrame) { float rawSetpoint = getRawSetpoint(axis); - + float absRawSetpoint = fabsf(rawSetpoint); const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; const float rxRate = 1.0f / rxInterval; float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate; - float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; - float setpointSpeedModified = setpointSpeed; - float setpointAccelerationModified = setpointAcceleration; + float absSetpointSpeed = fabsf(setpointSpeed); + float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]); + float setpointAccelerationModifier = 1.0f; - // Glitch reduction code for identical packets - if (fabsf(setpointAcceleration) > 3.0f * fabsf(prevAcceleration[axis])) { + if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) { + // no movement, or sticks at max; ffStatus set + // the max stick check is needed to prevent interpolation when arriving at max sticks + if (prevSetpointSpeed[axis] == 0) { + // no movement on two packets in a row + // do nothing now, but may use status = 3 to smooth following packet + ffStatus[axis] = 3; + } else { + // there was movement on previous packet, now none + if (bigStep[axis] == true) { + // previous movement was big; likely an early FrSky packet + // don't project these forward or we get a sustained large spike + ffStatus[axis] = 2; + } else { + // likely a dropped packet + // interpolate forward using previous setpoint speed and acceleration + setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis]; + // use status = 1 to halve the step for the next packet + ffStatus[axis] = 1; + } + } + } else { + // we have movement; let's consider what happened on previous packets, using ffStatus + if (ffStatus[axis] == 1) { + // was interpolated forward after previous dropped packet after small step + // this step is likely twice as tall as it should be + setpointSpeed = setpointSpeed / 2.0f; + } else if (ffStatus[axis] == 2) { + // we are doing nothing for these to avoid exaggerating the FrSky early packet problem + } else if (ffStatus[axis] == 3) { + // movement after nothing on previous two packets + // reduce boost when higher averaging is used to improve slow stick smoothness + setpointAccelerationModifier /= (averagingCount + 1); + } + ffStatus[axis] = 0; + // all is normal + } + + float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; + + // determine if this step was a relatively large one, to use when evaluating next packet + + if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){ bigStep[axis] = true; } else { bigStep[axis] = false; } - if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) { - // identical packet detected, not at full deflection. - // first packet on leaving full deflection always gets full FF - if (holdCount[axis] == 0) { - // previous packet had movement - if (bigStep[axis]) { - // type 1 = interpolate forward where acceleration change is large - setpointSpeedModified = prevSetpointSpeed[axis]; - setpointAccelerationModified = prevAcceleration[axis]; - holdCount[axis] = 1; - } else { - // type 2 = small change, no interpolation needed - setpointSpeedModified = 0.0f; - setpointSpeed = setpointSpeed / 2.0f; - holdCount[axis] = 2; - } - } else { - // it is an unchanged packet after previous unchanged packet - // speed and acceleration will be zero, no need to change anything - holdCount[axis] = 3; - } - } else { - // we're moving, or sticks are at max - if (holdCount[axis] != 0) { - // previous step was a duplicate, handle each type differently - if (holdCount[axis] == 1) { - // interpolation was applied - // raw setpoint speed of next 'good' packet is twice what it should be - setpointSpeedModified = setpointSpeed / 2.0f; - setpointSpeed = setpointSpeedModified; - // empirically this works best - setpointAccelerationModified = (prevAcceleration[axis] + setpointAcceleration) / 2.0f; - } else if (holdCount[axis] == 2) { - // interpolation was not applied - } else if (holdCount[axis] == 3) { - // after persistent flat period, no boost - // reduces jitter from boost when flying smooth lines - // but only when no ff_averaging is active, eg hard core race setups - // WARNING: this means no boost if ADC is active on FrSky radios - if (averagingCount > 1) { - setpointAccelerationModified /= averagingCount; - } - } - holdCount[axis] = 0; - } - } - // smooth deadband type suppression of FF jitter when sticks are at or returning to centre // only when ff_averaging is 3 or more, for HD or cinematic flying if (averagingCount > 2) { - const float rawSetpointCentred = fabsf(rawSetpoint) / averagingCount; + const float rawSetpointCentred = absRawSetpoint / averagingCount; if (rawSetpointCentred < 1.0f) { - setpointSpeedModified *= rawSetpointCentred; - setpointAccelerationModified *= rawSetpointCentred; - holdCount[axis] = 4; + setpointSpeed *= rawSetpointCentred; + setpointAcceleration *= rawSetpointCentred; } } - setpointDeltaImpl[axis] = setpointSpeedModified * pidGetDT(); prevAcceleration[axis] = setpointAcceleration; - + + // all values afterwards are small numbers setpointAcceleration *= pidGetDT(); - setpointAccelerationModified *= pidGetDT(); + setpointDeltaImpl[axis] = setpointSpeed * pidGetDT(); + const float ffBoostFactor = pidGetFfBoostFactor(); - float clip = 1.0f; float boostAmount = 0.0f; if (ffBoostFactor != 0.0f) { - //calculate clip factor to reduce boost on big spikes - if (pidGetSpikeLimitInverse()) { - clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse())); - clip *= clip; - } - // don't clip first step inwards from max deflection - if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) { - clip = 1.0f; - } // calculate boost and prevent kick-back spike at max deflection - if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) { - boostAmount = ffBoostFactor * setpointAccelerationModified; + if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { + boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier; } } @@ -172,18 +158,23 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp if (axis == FD_ROLL) { DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100)); - DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAccelerationModified * 100)); - DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpointAcceleration * 100)); - DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100)); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100))); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]); } - setpointDeltaImpl[axis] += boostAmount * clip; - - // first order (kind of) smoothing of FF + // first order smoothing of boost to reduce jitter const float ffSmoothFactor = pidGetFfSmoothFactor(); + boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]); + prevBoostAmount[axis] = boostAmount; + + setpointDeltaImpl[axis] += boostAmount; + + // first order smoothing of FF (second order boost filtering since boost filtered twice) setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]); prevDeltaImpl[axis] = setpointDeltaImpl[axis]; + // apply averaging if (type == FF_INTERPOLATE_ON) { setpointDelta[axis] = setpointDeltaImpl[axis]; } else { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 04efd92fdf..312ce8bd28 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 15); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 1); void resetPidProfile(pidProfile_t *pidProfile) { @@ -202,8 +202,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .idle_p = 50, .idle_pid_limit = 200, .idle_max_increase = 150, - .ff_interpolate_sp = FF_INTERPOLATE_AVG2, - .ff_spike_limit = 60, + .ff_interpolate_sp = FF_INTERPOLATE_ON, .ff_max_rate_limit = 100, .ff_smooth_factor = 37, .ff_boost = 15, @@ -246,12 +245,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -float pidGetSpikeLimitInverse() -{ - return pidRuntime.ffSpikeLimitInverse; -} - - float pidGetFfBoostFactor() { return pidRuntime.ffBoostFactor; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 887938092c..b2a7a3084b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -196,7 +196,6 @@ 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 uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode @@ -257,7 +256,6 @@ typedef struct pidRuntime_s { float antiGravityOsdCutoff; float antiGravityThrottleHpf; float ffBoostFactor; - float ffSpikeLimitInverse; float itermAccelerator; uint16_t itermAcceleratorGain; float feedForwardTransition; @@ -413,5 +411,4 @@ float pidGetDT(); float pidGetPidFrequency(); float pidGetFfBoostFactor(); float pidGetFfSmoothFactor(); -float pidGetSpikeLimitInverse(); float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 08545ee65b..fb624797f1 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -204,7 +204,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT)); pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f; - pidRuntime.ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f; } void pidInit(const pidProfile_t *pidProfile)