rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #11000 from ctzsnooze/feedforward-improved-handling-of-recurrent-duplicate-packets

This commit is contained in:
Michael Keller 2021-10-10 16:15:57 +13:00 committed by GitHub
parent ea3e64d49a 42a01e3e5b
commit 0b3a5a651b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 115 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
src/main/flight/feedforward.c
View File

@ -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) {