diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c index 10c1420ed..69c42a9a3 100644 --- a/src/main/flight/feedforward.c +++ b/src/main/flight/feedforward.c @@ -34,23 +34,20 @@ #include "feedforward.h" static float setpointDelta[XYZ_AXIS_COUNT]; +static float prevSetpoint[XYZ_AXIS_COUNT]; +static float prevSetpointSpeed[XYZ_AXIS_COUNT]; +static float prevAcceleration[XYZ_AXIS_COUNT]; +static uint8_t duplicateCount[XYZ_AXIS_COUNT]; +static uint8_t averagingCount; +static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT]; +static float feedforwardMaxRate[XYZ_AXIS_COUNT]; typedef struct laggedMovingAverageCombined_s { laggedMovingAverage_t filter; float buf[4]; } laggedMovingAverageCombined_t; - laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT]; -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 uint8_t goodPacketCount[XYZ_AXIS_COUNT]; -static uint8_t averagingCount; -static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT]; -static float feedforwardMaxRate[XYZ_AXIS_COUNT]; - void feedforwardInit(const pidProfile_t *pidProfile) { const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f; averagingCount = pidProfile->feedforward_averaging + 1; @@ -64,110 +61,134 @@ void feedforwardInit(const pidProfile_t *pidProfile) { FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) { if (newRcFrame) { - float rcCommandDelta = getRcCommandDelta(axis); - float setpoint = getRawSetpoint(axis); - const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; - 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 feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor(); const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor(); + // good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor(); - float feedforward; - - 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); - // calculate the jitter attenuator from average of two most recent abs rcCommand deltas vs jitter threshold - 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) { - // duplicate packet data on this axis - if (getRxRateValid() && goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) { - // interpolate if two or more preceding valid packets, or if sticks near max - setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator; - // setpoint interpolation includes previous acceleration and attenuation - setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; - // recalculate speed and acceleration based on new setpoint - } else { - // force to zero - setpointSpeed = 0.0f; - jitterAttenuator = 0.0f; - } - goodPacketCount[axis] = 0; - } else { - // we have movement - // count the number of valid steps to decide if we permit interpolation, for Tx ADC filter situations especially - if (goodPacketCount[axis] < 2) { - goodPacketCount[axis] += 1; - } - } - - prevSetpoint[axis] = setpoint; - - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint)); // setpoint after interpolations - } - - // first order type smoothing for derivative - setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); - - // second order smoothing for for acceleration by calculating it after smoothing setpointSpeed - setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; - setpointAcceleration *= jitterAttenuator * 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(); - feedforward = setpointSpeed * pidGetDT(); - - // calculate boost and prevent kick-back spike at max deflection + // 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor(); - float boostAmount = 0.0f; - if (feedforwardBoostFactor) { - if (setpointPercent < 0.95f || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) { - // allow boost when returning from max, but not when hitting max on the way up - boostAmount = feedforwardBoostFactor * setpointAcceleration; + + const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link. + const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link + + const float setpoint = getRawSetpoint(axis); + const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis]; + + float rcCommandDelta = getRcCommandDelta(axis); + + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f)); + // rcCommand packet difference = value of 100 if 1000 RC steps + DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint)); + // un-smoothed in blackbox + } + + // calculate setpoint speed + float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; + float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention + float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]); + + float setpointAcceleration = 0.0f; + + rcCommandDelta = fabsf(rcCommandDelta); + + if (rcCommandDelta) { + // we have movement and should calculate feedforward + + // jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold + float jitterAttenuator = 1.0f; + if (feedforwardJitterFactor) { + if (rcCommandDelta < feedforwardJitterFactor) { + jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f); + jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator; + } + } + + // duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet + // reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero. + // needed because while sticks are moving, the next valid step up will be proportionally bigger + // and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active + // downside is that for truly held sticks, the first feedforward step won't be as big as it should be + if (duplicateCount[axis]) { + setpointSpeed /= duplicateCount[axis] + 1; + } + + // first order type smoothing for setpoint speed noise reduction + setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); + + // calculate acceleration from smoothed setpoint speed + setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; + + // use rxRate to normalise acceleration to nominal RC packet interval of 100hz + // without this, we would get less boost than we should at higher Rx rates + // note rxRate updates with every new packet (though not every time data changes), hence + // if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay + setpointAcceleration *= rxRate * 0.01f; + + // first order acceleration smoothing (with smoothed input this is effectively second order all up) + setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]); + + // jitter reduction to reduce acceleration spikes at low rcCommandDelta values + // no effect for rcCommandDelta values above jitter threshold (zero delay) + // does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo + setpointAcceleration *= jitterAttenuator; + + if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) { + // approaching max stick position so zero out feedforward to minimise overshoot + setpointSpeed = 0.0f; + setpointAcceleration = 0.0f; + } + + prevSetpointSpeed[axis] = setpointSpeed; + prevAcceleration[axis] = setpointAcceleration; + + setpointAcceleration *= feedforwardBoostFactor; + + // add attenuated boost to base feedforward and apply jitter attenuation + setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator; + + //reset counter + duplicateCount[axis] = 0; + + } else { + // no movement + if (duplicateCount[axis]) { + // increment duplicate count to max of 2 + duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0; + // second or subsequent duplicate, or duplicate when held at max stick or centre position. + // force feedforward to zero + setpointDelta[axis] = 0.0f; + // zero speed and acceleration for correct smoothing of next good packet + setpointSpeed = 0.0f; + prevSetpointSpeed[axis] = 0.0f; + prevAcceleration[axis] = 0.0f; + } else { + // first duplicate; hold feedforward and previous static values, as if we just never got anything + duplicateCount[axis] = 1; } } + if (axis == FD_ROLL) { - 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_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing + DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing // debug 0 is interpolated setpoint, above // debug 3 is rcCommand delta, above } - // add attenuated boost to base feedforward - feedforward += boostAmount; + prevSetpoint[axis] = setpoint; - // apply averaging, if enabled + // apply averaging, if enabled - include zero values in averaging if (feedforwardAveraging) { - setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, feedforward); - } else { - setpointDelta[axis] = feedforward; + setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]); } // apply feedforward transition setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f; } - return setpointDelta[axis]; + return setpointDelta[axis]; // the value used by the PID code } FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) {