diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 1a752db76..be19a2307 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -91,8 +91,8 @@ const char * const debugModeNames[DEBUG_COUNT] = { "BARO", "GPS_RESCUE_THROTTLE_PID", "DYN_IDLE", - "FF_LIMIT", - "FF_INTERPOLATED", + "FEEDFORWARD_LIMIT", + "FEEDFORWARD", "BLACKBOX_OUTPUT", "GYRO_SAMPLE", "RX_TIMING", diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 285977efb..425cbb852 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -107,8 +107,8 @@ typedef enum { DEBUG_BARO, DEBUG_GPS_RESCUE_THROTTLE_PID, DEBUG_DYN_IDLE, - DEBUG_FF_LIMIT, - DEBUG_FF_INTERPOLATED, + DEBUG_FEEDFORWARD_LIMIT, + DEBUG_FEEDFORWARD, DEBUG_BLACKBOX_OUTPUT, DEBUG_GYRO_SAMPLE, DEBUG_RX_TIMING, diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9c143a0b4..8d2675d19 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1151,7 +1151,7 @@ const clivalue_t valueTable[] = { #ifdef USE_FEEDFORWARD { "feedforward_averaging", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) }, - { "feedforward_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) }, + { "feedforward_smoothing", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) }, { "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) }, { "feedforward_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) }, #endif diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 15d988a82..89250475a 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -554,7 +554,7 @@ FAST_CODE void processRcCommand(void) for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]); - rcCommandDelta[axis] = fabsf(rcCommand[axis] - oldRcCommand[axis]); + rcCommandDelta[axis] = (rcCommand[axis] - oldRcCommand[axis]); oldRcCommand[axis] = rcCommand[axis]; float angleRate; diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c index 3c384fbab..af9ca3d4f 100644 --- a/src/main/flight/feedforward.c +++ b/src/main/flight/feedforward.c @@ -33,7 +33,6 @@ #include "feedforward.h" -static float setpointDeltaImpl[XYZ_AXIS_COUNT]; static float setpointDelta[XYZ_AXIS_COUNT]; typedef struct laggedMovingAverageCombined_s { @@ -43,23 +42,21 @@ typedef struct laggedMovingAverageCombined_s { laggedMovingAverageCombined_t setpointDeltaAvg[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 float prevSetpoint[XYZ_AXIS_COUNT]; +static float prevSetpointSpeed[XYZ_AXIS_COUNT]; +static float prevAcceleration[XYZ_AXIS_COUNT]; +static float prevRcCommandDelta[XYZ_AXIS_COUNT]; +static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; static uint8_t averagingCount; - -static float ffMaxRateLimit[XYZ_AXIS_COUNT]; -static float ffMaxRate[XYZ_AXIS_COUNT]; +static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT]; +static float feedforwardMaxRate[XYZ_AXIS_COUNT]; void feedforwardInit(const pidProfile_t *pidProfile) { - const float ffMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f; + const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f; averagingCount = pidProfile->feedforward_averaging + 1; for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - ffMaxRate[i] = applyCurve(i, 1.0f); - ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale; + feedforwardMaxRate[i] = applyCurve(i, 1.0f); + feedforwardMaxRateLimit[i] = feedforwardMaxRate[i] * feedforwardMaxRateScale; laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]); } } @@ -70,88 +67,96 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward float rcCommandDelta = getRcCommandDelta(axis); float setpoint = getRawSetpoint(axis); const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; - const float rxRate = 1.0f / rxInterval; + const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]); float setpointAcceleration = 0.0f; - const float ffSmoothFactor = pidGetFfSmoothFactor(); - const float ffJitterFactor = pidGetFfJitterFactor(); + const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor(); + const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor(); + float feedforward; // 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; + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference = steps of 50 mean 2000 RC steps + } + rcCommandDelta = fabsf(rcCommandDelta); + float jitterAttenuator = 1.0f; + if (feedforwardJitterFactor) { + if (rcCommandDelta < feedforwardJitterFactor) { + jitterAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / feedforwardJitterFactor, 0.0f); + jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator; } } + const float setpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis]; + float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention + // 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; + if (prevDuplicatePacket[axis] == false) { + // first duplicate after movement, interpolate setpoint and use previous acceleration + // don't interpolate if sticks close to centre or max, interpolate jitter signals less than larger ones + if (setpointPercent > 0.02f && setpointPercent < 0.95f) { + // setpoint interpolation includes previous acceleration and attenuation + setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator ) * rxInterval * jitterAttenuator; + // recalculate speed and acceleration + setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; + } + } else { + // force to zero + setpointSpeed = 0.0f; } prevDuplicatePacket[axis] = true; } else { - // movement! - if (prevDuplicatePacket[axis] == true) { - // don't boost the packet after a duplicate, the feedforward 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; if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations + DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint)); // setpoint after interpolations } - float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention + // first order type smoothing for derivative + setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); - // calculate acceleration, smooth and attenuate it + // second order smoothing for for acceleration by calculating it after smoothing setpointSpeed setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; - setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]); - setpointAcceleration *= ffAttenuator; - - // smooth setpointSpeed but don't attenuate - setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); + setpointAcceleration *= rxRate * 0.01f; // adjust boost for RC packet interval, including dropped packets + setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]); prevSetpointSpeed[axis] = setpointSpeed; prevAcceleration[axis] = setpointAcceleration; prevRcCommandDelta[axis] = rcCommandDelta; setpointAcceleration *= pidGetDT(); - setpointDeltaImpl[axis] = setpointSpeed * pidGetDT(); + feedforward = setpointSpeed * pidGetDT(); // calculate boost and prevent kick-back spike at max deflection - const float ffBoostFactor = pidGetFfBoostFactor(); + const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor(); float boostAmount = 0.0f; - if (ffBoostFactor) { - if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { - boostAmount = ffBoostFactor * setpointAcceleration; + if (feedforwardBoostFactor) { + // allows boost when returning from max, but not when hitting max on the way up + if (setpointPercent < 0.95f || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { + boostAmount = feedforwardBoostFactor * setpointAcceleration * jitterAttenuator; } } if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base feedforward - 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 + DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(feedforward * 100.0f)); // delta after interpolating duplicates and smoothing + DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(boostAmount * 100.0f)); // boost amount after jitter reduction and smoothing + // debug 0 is interpolated setpoint, above + // debug 3 is rcCommand delta, above } - // add boost to base feedforward - setpointDeltaImpl[axis] += boostAmount; + // add attenuated boost to base feedforward + feedforward += boostAmount; - // apply averaging + // apply averaging, if enabled if (feedforwardAveraging) { - setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]); + setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, feedforward); } else { - setpointDelta[axis] = setpointDeltaImpl[axis]; + setpointDelta[axis] = feedforward; } } return setpointDelta[axis]; @@ -160,23 +165,23 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) { switch (axis) { case FD_ROLL: - DEBUG_SET(DEBUG_FF_LIMIT, 0, value); - + DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value); break; case FD_PITCH: - DEBUG_SET(DEBUG_FF_LIMIT, 1, value); - + DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value); break; } - if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) { - value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp); - } else { - value = 0; + if (value * currentPidSetpoint > 0.0f) { + if (fabsf(currentPidSetpoint) <= feedforwardMaxRateLimit[axis]) { + value = constrainf(value, (-feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp, (feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp); + } else { + value = 0; + } } if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_FF_LIMIT, 2, value); + DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value); } return value; @@ -184,6 +189,6 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, bool shouldApplyFeedforwardLimits(int axis) { - return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW; + return feedforwardMaxRateLimit[axis] != 0.0f && axis < FD_YAW; } #endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a76452305..f9d14886d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -204,7 +204,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dyn_idle_max_increase = 150, .feedforward_averaging = FEEDFORWARD_AVERAGING_OFF, .feedforward_max_rate_limit = 100, - .feedforward_smooth_factor = 37, + .feedforward_smooth_factor = 25, .feedforward_jitter_factor = 7, .feedforward_boost = 15, .dyn_lpf_curve_expo = 5, @@ -256,20 +256,20 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -float pidGetFfBoostFactor() +float pidGetFeedforwardBoostFactor() { - return pidRuntime.ffBoostFactor; + return pidRuntime.feedforwardBoostFactor; } #ifdef USE_FEEDFORWARD -float pidGetFfSmoothFactor() +float pidGetFeedforwardSmoothFactor() { - return pidRuntime.ffSmoothFactor; + return pidRuntime.feedforwardSmoothFactor; } -float pidGetFfJitterFactor() +float pidGetFeedforwardJitterFactor() { - return pidRuntime.ffJitterFactor; + return pidRuntime.feedforwardJitterFactor; } #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 009b28930..f99140cba 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -284,7 +284,7 @@ typedef struct pidRuntime_s { float antiGravityOsdCutoff; float antiGravityThrottleHpf; float antiGravityPBoost; - float ffBoostFactor; + float feedforwardBoostFactor; float itermAccelerator; uint16_t itermAcceleratorGain; float feedforwardTransition; @@ -386,8 +386,8 @@ typedef struct pidRuntime_s { #ifdef USE_FEEDFORWARD feedforwardAveraging_t feedforwardAveraging; - float ffSmoothFactor; - float ffJitterFactor; + float feedforwardSmoothFactor; + float feedforwardJitterFactor; #endif } pidRuntime_t; @@ -440,7 +440,7 @@ void pidSetItermReset(bool enabled); float pidGetPreviousSetpoint(int axis); float pidGetDT(); float pidGetPidFrequency(); -float pidGetFfBoostFactor(); -float pidGetFfSmoothFactor(); -float pidGetFfJitterFactor(); +float pidGetFeedforwardBoostFactor(); +float pidGetFeedforwardSmoothFactor(); +float pidGetFeedforwardJitterFactor(); 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 458a37803..72243eadf 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -239,7 +239,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT)); pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT)); - pidRuntime.ffBoostFactor = (float)pidProfile->feedforward_boost / 10.0f; + pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f; } void pidInit(const pidProfile_t *pidProfile) @@ -423,13 +423,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_FEEDFORWARD pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging; + pidRuntime.feedforwardSmoothFactor = 1.0f; if (pidProfile->feedforward_smooth_factor) { - pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f; - } else { - // set automatically according to boost amount, limit to 0.5 for auto - pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->feedforward_boost) * 2.0f / 100.0f); + pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f; } - pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor; + pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor; feedforwardInit(pidProfile); #endif