Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

stop feedback in feedforward, and improve noise, when Tx ADC filter is enabled

Browse source
This commit is contained in:
ctzsnooze 2021-08-07 22:53:43 +10:00
parent 55f2be55f6
commit 91191f4b62
1 changed files with 18 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

29
src/main/flight/feedforward.c
View file

@ -46,7 +46,7 @@ static float prevSetpoint[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT]; static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevAcceleration[XYZ_AXIS_COUNT]; static float prevAcceleration[XYZ_AXIS_COUNT];
static float prevRcCommandDelta[XYZ_AXIS_COUNT]; static float prevRcCommandDelta[XYZ_AXIS_COUNT];
static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; static uint8_t goodPacketCount[XYZ_AXIS_COUNT];
static uint8_t averagingCount; static uint8_t averagingCount;
static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT]; static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT];
static float feedforwardMaxRate[XYZ_AXIS_COUNT]; static float feedforwardMaxRate[XYZ_AXIS_COUNT];
@ -93,22 +93,25 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
// interpolate setpoint if necessary // interpolate setpoint if necessary
if (rcCommandDelta == 0.0f) { if (rcCommandDelta == 0.0f) {
if (prevDuplicatePacket[axis] == false) { // duplicate packet data on this axis
// first duplicate after movement, interpolate setpoint and use previous acceleration if (goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) {
// don't interpolate if sticks close to centre or max, interpolate jitter signals less than larger ones // interpolate if two or more preceding valid packets, or if sticks near max
if (setpointPercent > 0.02f && setpointPercent < 0.95f) {
// setpoint interpolation includes previous acceleration and attenuation
setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator; setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator;
// recalculate speed and acceleration // setpoint interpolation includes previous acceleration and attenuation
setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate; setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
} // recalculate speed and acceleration based on new setpoint
} else { } else {
// force to zero // force to zero
setpointSpeed = 0.0f; setpointSpeed = 0.0f;
jitterAttenuator = 0.0f;
} }
prevDuplicatePacket[axis] = true; goodPacketCount[axis] = 0;
} else { } else {
prevDuplicatePacket[axis] = false; // 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; prevSetpoint[axis] = setpoint;
@ -122,7 +125,7 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
// second order smoothing for for acceleration by calculating it after smoothing setpointSpeed // second order smoothing for for acceleration by calculating it after smoothing setpointSpeed
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
setpointAcceleration *= rxRate * 0.01f; // adjust boost for RC packet interval, including dropped packets setpointAcceleration *= jitterAttenuator * rxRate * 0.01f; // adjust boost for RC packet interval, including dropped packets
setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]); setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
prevSetpointSpeed[axis] = setpointSpeed; prevSetpointSpeed[axis] = setpointSpeed;
@ -136,9 +139,9 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor(); const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
float boostAmount = 0.0f; float boostAmount = 0.0f;
if (feedforwardBoostFactor) { 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) { if (setpointPercent < 0.95f || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
boostAmount = feedforwardBoostFactor * setpointAcceleration * jitterAttenuator; // allow boost when returning from max, but not when hitting max on the way up
boostAmount = feedforwardBoostFactor * setpointAcceleration;
} }
} }