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

fixes for feedforward for 4.3

This commit is contained in:
ctzsnooze 2021-08-03 23:32:34 +10:00
parent 864cf3f3b4
commit 45ff9ea1e5
8 changed files with 93 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/build/debug.c
View File

@ -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",

4
src/main/build/debug.h
View File

@ -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,

2
src/main/cli/settings.c
View File

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

2
src/main/fc/rc.c
View File

@ -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;

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

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

14
src/main/flight/pid.c
View File

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

12
src/main/flight/pid.h
View File

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

10
src/main/flight/pid_init.c
View File

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