Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Interpolated feed forward smoothing

Browse source
This commit is contained in:
ctzsnooze 2019-11-20 11:28:04 +11:00
parent 881a256980
commit 3c91da0e59
5 changed files with 85 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/cli/settings.c
View file

@ -476,7 +476,7 @@ static const char * const lookupTableOffOnAuto[] = {
};
static const char* const lookupTableInterpolatedSetpoint[] = {
"OFF", "ON", "AVERAGED"
"OFF", "ON", "AVERAGED_2", "AVERAGED_3", "AVERAGED_4"
};
static const char* const lookupTableDshotBitbangedTimer[] = {
@ -1097,6 +1097,7 @@ const clivalue_t valueTable[] = {
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_spike_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spike_limit) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
{ "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },

79
src/main/flight/interpolated_setpoint.c
View file

@ -28,15 +28,24 @@
#include "fc/rc.h"
#include "flight/interpolated_setpoint.h"
#define PREV_BIG_STEP 1000.0f //threshold for size of jump of packet before the identical data packet
static float setpointDeltaImpl[XYZ_AXIS_COUNT];
static float prevSetpointDeltaImpl[XYZ_AXIS_COUNT];
static float setpointDelta[XYZ_AXIS_COUNT];
static uint8_t holdCount[XYZ_AXIS_COUNT];
typedef struct laggedMovingAverageCombined_u {
laggedMovingAverage_t filter;
float buf[4];
} laggedMovingAverageCombined_t;
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevAcceleration[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
static float prevSetpointAcceleration[XYZ_AXIS_COUNT];
static float prevDeltaImpl[XYZ_AXIS_COUNT];
static bool bigStep[XYZ_AXIS_COUNT];
// Configuration
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@ -44,9 +53,11 @@ static float ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
uint8_t j = pidProfile->ff_interpolate_sp;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f);
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, j, (float *)&setpointDeltaAvg[i].buf[0]);
}
}
@ -58,9 +69,46 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
const float rxInterval = currentRxRefreshRate * 1e-6f;
const float rxRate = 1.0f / rxInterval;
const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
const uint8_t holdSteps = 2;
// Glitch reduction code for identical packets
if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) {
// identical packets, not at full deflection
if (holdCount[axis] < holdSteps && fabsf(rawSetpoint) > 2.0f && !bigStep[axis]) {
// holding the entire previous speed is best for missed packets, but bad for early packets
setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
setpointAcceleration = prevAcceleration[axis];
holdCount[axis] += 1;
} else {
// identical packets for more than hold steps, or prev big step
// lock acceleration to zero and don't interpolate forward until sticks move again
holdCount[axis] = holdSteps + 1;
setpointAcceleration = 0.0f;
}
} else {
// we're moving, or sticks are at max
if (holdCount[axis] == 2) {
// we are after an identical packet, and the one before was a normal step up,
// so raw step speed and acceleration of next 'good' packet is twice what it should be
setpointSpeed /= 2.0f;
setpointAcceleration /= 2.0f;
}
if (holdCount[axis] == 3) {
// we are starting to move after a gap after a big step up, or persistent flat period, so accelerate gently
setpointAcceleration = 0.0f;
}
holdCount[axis] = 1;
if (fabsf(setpointAcceleration - prevAcceleration[axis]) > PREV_BIG_STEP) {
bigStep[axis] = true;
} else {
bigStep[axis] = false;
}
}
prevAcceleration[axis] = setpointAcceleration;
setpointAcceleration *= pidGetDT();
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
const float ffBoostFactor = pidGetFfBoostFactor();
@ -75,25 +123,34 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
// no clip for first step inwards from max deflection
if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
clip = 1.0f;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevSetpointAcceleration[axis] = setpointAcceleration;
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, boostAmount * clip * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, clip * 100);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, bigStep[axis]);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]);
}
setpointDeltaImpl[axis] += boostAmount * clip;
// first order filter FF
const float ffSmoothFactor = pidGetFfSmoothFactor();
setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
prevDeltaImpl[axis] = setpointDeltaImpl[axis];
if (type == FF_INTERPOLATE_ON) {
setpointDelta[axis] = setpointDeltaImpl[axis];
} else {
setpointDelta[axis] = 0.5f * (setpointDeltaImpl[axis] + prevSetpointDeltaImpl[axis]);
prevSetpointDeltaImpl[axis] = setpointDeltaImpl[axis];
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
}
}
return setpointDelta[axis];
}

4
src/main/flight/interpolated_setpoint.h
View file

@ -28,7 +28,9 @@
typedef enum ffInterpolationType_e {
FF_INTERPOLATE_OFF,
FF_INTERPOLATE_ON,
FF_INTERPOLATE_AVG
FF_INTERPOLATE_AVG2,
FF_INTERPOLATE_AVG3,
FF_INTERPOLATE_AVG4
} ffInterpolationType_t;
void interpolatedSpInit(const pidProfile_t *pidProfile);

12
src/main/flight/pid.c
View file

@ -212,9 +212,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_p = 50,
.idle_pid_limit = 200,
.idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_AVG,
.ff_spike_limit = 60,
.ff_interpolate_sp = FF_INTERPOLATE_AVG2,
.ff_spike_limit = 50,
.ff_max_rate_limit = 100,
.ff_smooth_factor = 37,
.ff_boost = 15,
);
#ifndef USE_D_MIN
@ -314,6 +315,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
static FAST_RAM_ZERO_INIT float ffBoostFactor;
static FAST_RAM_ZERO_INIT float ffSmoothFactor;
static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse;
float pidGetSpikeLimitInverse()
@ -327,6 +329,11 @@ float pidGetFfBoostFactor()
return ffBoostFactor;
}
float pidGetFfSmoothFactor()
{
return ffSmoothFactor;
}
void pidInitFilters(const pidProfile_t *pidProfile)
{
@ -738,6 +745,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#ifdef USE_INTERPOLATED_SP
ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
interpolatedSpInit(pidProfile);
#endif
}

2
src/main/flight/pid.h
View file

@ -184,6 +184,7 @@ typedef struct pidProfile_s {
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -261,4 +262,5 @@ float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetFfSmoothFactor();
float pidGetSpikeLimitInverse();