From d4f0ec2d0a3285e14f49383cf14060457d88d0bb Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 27 Apr 2021 08:20:35 +1000 Subject: [PATCH] feed forward jitter improvements --- src/main/cli/settings.c | 1 + src/main/cms/cms_menu_imu.c | 4 + src/main/fc/rc.c | 12 +++ src/main/fc/rc.h | 1 + src/main/flight/interpolated_setpoint.c | 149 +++++++++++++------------------- src/main/flight/pid.c | 8 +- src/main/flight/pid.h | 3 + src/main/flight/pid_init.c | 1 + 8 files changed, 89 insertions(+), 90 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 698bd8b1f..a76d7ebfe 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1164,6 +1164,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_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) }, + { "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_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/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 616069820..d0ccabd63 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -519,6 +519,7 @@ static uint8_t cmsx_iterm_relax_cutoff; #ifdef USE_INTERPOLATED_SP static uint8_t cmsx_ff_interpolate_sp; static uint8_t cmsx_ff_smooth_factor; +static uint8_t cmsx_ff_jitter_factor; #endif static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp) @@ -561,6 +562,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp) #ifdef USE_INTERPOLATED_SP cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp; cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor; + cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor; #endif #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION @@ -608,6 +610,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry #ifdef USE_INTERPOLATED_SP pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp; pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor; + pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor; #endif #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION @@ -625,6 +628,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { #ifdef USE_INTERPOLATED_SP { "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 }, { "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 }, + { "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 }, #endif { "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 1af4dbdd9..a8900a6eb 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -61,7 +61,11 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom #ifdef USE_INTERPOLATED_SP // Setpoint in degrees/sec before RC-Smoothing is applied static float rawSetpoint[XYZ_AXIS_COUNT]; +static float oldRcCommand[XYZ_AXIS_COUNT]; +static bool isDuplicate[XYZ_AXIS_COUNT]; +float rcCommandDelta[XYZ_AXIS_COUNT]; #endif + static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; static bool reverseMotors = false; @@ -132,6 +136,11 @@ float getRawSetpoint(int axis) return rawSetpoint[axis]; } +float getRcCommandDelta(int axis) +{ + return rcCommandDelta[axis]; +} + #endif #define THROTTLE_LOOKUP_LENGTH 12 @@ -636,6 +645,9 @@ FAST_CODE void processRcCommand(void) #ifdef USE_INTERPOLATED_SP if (isRxDataNew) { for (int i = FD_ROLL; i <= FD_YAW; i++) { + isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]); + rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]); + oldRcCommand[i] = rcCommand[i]; float rcCommandf; if (i == FD_YAW) { rcCommandf = rcCommand[i] / rcCommandYawDivider; diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index bc9068c0b..f678b9ed1 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -51,6 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void); bool rcSmoothingAutoCalculate(void); bool rcSmoothingInitializationComplete(void); float getRawSetpoint(int axis); +float getRcCommandDelta(int axis); float applyCurve(int axis, float deflection); bool getShouldUpdateFf(); void updateRcRefreshRate(timeUs_t currentTimeUs); diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c index 5869dbe7e..a92f0f924 100644 --- a/src/main/flight/interpolated_setpoint.c +++ b/src/main/flight/interpolated_setpoint.c @@ -43,15 +43,12 @@ typedef struct laggedMovingAverageCombined_s { 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 float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated +static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated +static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation +static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation + +static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets static uint8_t averagingCount; static float ffMaxRateLimit[XYZ_AXIS_COUNT]; @@ -70,112 +67,86 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) { FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) { if (newRcFrame) { - float rawSetpoint = getRawSetpoint(axis); - float absRawSetpoint = fabsf(rawSetpoint); + float rcCommandDelta = getRcCommandDelta(axis); + float setpoint = getRawSetpoint(axis); const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; const float rxRate = 1.0f / rxInterval; - float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate; - float absSetpointSpeed = fabsf(setpointSpeed); + float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]); - float setpointAccelerationModifier = 1.0f; - - 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; - } + float setpointAcceleration = 0.0f; + const float ffSmoothFactor = pidGetFfSmoothFactor(); + const float ffJitterFactor = pidGetFfJitterFactor(); + + // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold + float ffAttenuator = 1.0f; + if (ffJitterFactor) { + if (rcCommandDelta < ffJitterFactor) { + ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f); + ffAttenuator = 1.0f - ffAttenuator * ffAttenuator; + } + } + + // interpolate setpoint if necessary + if (rcCommandDelta == 0.0f) { + if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) { + // first duplicate after movement + // interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint + setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval; + // recalculate setpointSpeed and (later) acceleration from this new setpoint value + setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; } + prevDuplicatePacket[axis] = true; } else { - // we have movement; let's consider what happened on previous packets, using ffStatus - if (ffStatus[axis] != 0) { - 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 + // movement! + if (prevDuplicatePacket[axis] == true) { + // don't boost the packet after a duplicate, the FF alone is enough, usually + // in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active + ffAttenuator = 0.0f; } + prevDuplicatePacket[axis] = false; } + prevSetpoint[axis] = setpoint; - float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations + } - // determine if this step was a relatively large one, to use when evaluating next packet + float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention - if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){ - bigStep[axis] = true; - } else { - bigStep[axis] = false; - } + // calculate acceleration, smooth and attenuate it + setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; + setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]); + setpointAcceleration *= ffAttenuator; - // 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 = absRawSetpoint / averagingCount; - if (rawSetpointCentred < 1.0f) { - setpointSpeed *= rawSetpointCentred; - setpointAcceleration *= rawSetpointCentred; - } - } + // smooth setpointSpeed but don't attenuate + setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); + prevSetpointSpeed[axis] = setpointSpeed; prevAcceleration[axis] = setpointAcceleration; - - // all values afterwards are small numbers + prevRcCommandDelta[axis] = rcCommandDelta; + setpointAcceleration *= pidGetDT(); setpointDeltaImpl[axis] = setpointSpeed * pidGetDT(); - + // calculate boost and prevent kick-back spike at max deflection const float ffBoostFactor = pidGetFfBoostFactor(); float boostAmount = 0.0f; - if (ffBoostFactor != 0.0f) { - // calculate boost and prevent kick-back spike at max deflection - if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { - boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier; + if (ffBoostFactor) { + if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { + boostAmount = ffBoostFactor * setpointAcceleration; } } - prevSetpointSpeed[axis] = setpointSpeed; - prevRawSetpoint[axis] = rawSetpoint; - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100)); - 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]); + DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF + DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount + // debug 2 is interpolated setpoint, above + DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference } - // first order smoothing of boost to reduce jitter - const float ffSmoothFactor = pidGetFfSmoothFactor(); - boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]); - prevBoostAmount[axis] = boostAmount; - + // add boost to base feed forward 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]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 461bb3a15..6c2523be1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -205,7 +205,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .dyn_idle_max_increase = 150, .ff_interpolate_sp = FF_INTERPOLATE_ON, .ff_max_rate_limit = 100, - .ff_smooth_factor = 0, + .ff_smooth_factor = 37, + .ff_jitter_factor = 7, .ff_boost = 15, .dyn_lpf_curve_expo = 5, .level_race_mode = false, @@ -266,6 +267,11 @@ float pidGetFfSmoothFactor() { return pidRuntime.ffSmoothFactor; } + +float pidGetFfJitterFactor() +{ + return pidRuntime.ffJitterFactor; +} #endif void pidResetIterm(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 786b9b593..d0f0f4d04 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -210,6 +210,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_smooth_factor; // Amount of smoothing for interpolated FF steps + uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF 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 uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount @@ -385,6 +386,7 @@ typedef struct pidRuntime_s { #ifdef USE_INTERPOLATED_SP ffInterpolationType_t ffFromInterpolatedSetpoint; float ffSmoothFactor; + float ffJitterFactor; #endif } pidRuntime_t; @@ -439,4 +441,5 @@ float pidGetDT(); float pidGetPidFrequency(); float pidGetFfBoostFactor(); float pidGetFfSmoothFactor(); +float pidGetFfJitterFactor(); 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 787759315..4153cb955 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -392,6 +392,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) // set automatically according to boost amount, limit to 0.5 for auto pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f); } + pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor; interpolatedSpInit(pidProfile); #endif -- 2.11.4.GIT