Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

feed forward jitter improvements

Browse source
This commit is contained in:
ctzsnooze 2021-04-27 08:20:35 +10:00
parent 3ae7e917b7
commit d4f0ec2d0a
8 changed files with 88 additions and 89 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1164,6 +1164,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_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) },
{ "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },

4
src/main/cms/cms_menu_imu.c
View file

@ -519,6 +519,7 @@ static uint8_t cmsx_iterm_relax_cutoff;
#ifdef USE_INTERPOLATED_SP
static uint8_t cmsx_ff_interpolate_sp;
static uint8_t cmsx_ff_smooth_factor;
static uint8_t cmsx_ff_jitter_factor;
#endif
static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
@ -561,6 +562,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
#ifdef USE_INTERPOLATED_SP
cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor;
#endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -608,6 +610,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
#ifdef USE_INTERPOLATED_SP
pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor;
#endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -625,6 +628,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
#ifdef USE_INTERPOLATED_SP
{ "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 },
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 },
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 },
#endif
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },

12
src/main/fc/rc.c
View file

@ -61,7 +61,11 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
#ifdef USE_INTERPOLATED_SP
// Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicate[XYZ_AXIS_COUNT];
float rcCommandDelta[XYZ_AXIS_COUNT];
#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
@ -132,6 +136,11 @@ float getRawSetpoint(int axis)
return rawSetpoint[axis];
}
float getRcCommandDelta(int axis)
{
return rcCommandDelta[axis];
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12
@ -636,6 +645,9 @@ FAST_CODE void processRcCommand(void)
#ifdef USE_INTERPOLATED_SP
if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
oldRcCommand[i] = rcCommand[i];
float rcCommandf;
if (i == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider;

1
src/main/fc/rc.h
View file

@ -51,6 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis);
float getRcCommandDelta(int axis);
float applyCurve(int axis, float deflection);
bool getShouldUpdateFf();
void updateRcRefreshRate(timeUs_t currentTimeUs);

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

@ -43,15 +43,12 @@ typedef struct laggedMovingAverageCombined_s {
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevAcceleration[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
//for smoothing
static float prevDeltaImpl[XYZ_AXIS_COUNT];
static float prevBoostAmount[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 uint8_t ffStatus[XYZ_AXIS_COUNT];
static bool bigStep[XYZ_AXIS_COUNT];
static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
static uint8_t averagingCount;
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@ -70,112 +67,86 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) {
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
if (newRcFrame) {
float rawSetpoint = getRawSetpoint(axis);
float absRawSetpoint = fabsf(rawSetpoint);
float rcCommandDelta = getRcCommandDelta(axis);
float setpoint = getRawSetpoint(axis);
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
const float rxRate = 1.0f / rxInterval;
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
float absSetpointSpeed = fabsf(setpointSpeed);
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
float setpointAccelerationModifier = 1.0f;
float setpointAcceleration = 0.0f;
const float ffSmoothFactor = pidGetFfSmoothFactor();
const float ffJitterFactor = pidGetFfJitterFactor();
if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) {
// no movement, or sticks at max; ffStatus set
// the max stick check is needed to prevent interpolation when arriving at max sticks
if (prevSetpointSpeed[axis] == 0) {
// no movement on two packets in a row
// do nothing now, but may use status = 3 to smooth following packet
ffStatus[axis] = 3;
} else {
// there was movement on previous packet, now none
if (bigStep[axis] == true) {
// previous movement was big; likely an early FrSky packet
// don't project these forward or we get a sustained large spike
ffStatus[axis] = 2;
} else {
// likely a dropped packet
// interpolate forward using previous setpoint speed and acceleration
setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
// use status = 1 to halve the step for the next packet
ffStatus[axis] = 1;
}
// 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;
}
}
// 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;
}
prevDuplicatePacket[axis] = true;
} else {
// we have movement; let's consider what happened on previous packets, using ffStatus
if (ffStatus[axis] != 0) {
if (ffStatus[axis] == 1) {
// was interpolated forward after previous dropped packet after small step
// this step is likely twice as tall as it should be
setpointSpeed = setpointSpeed / 2.0f;
} else if (ffStatus[axis] == 2) {
// we are doing nothing for these to avoid exaggerating the FrSky early packet problem
} else if (ffStatus[axis] == 3) {
// movement after nothing on previous two packets
// reduce boost when higher averaging is used to improve slow stick smoothness
setpointAccelerationModifier /= (averagingCount + 1);
}
ffStatus[axis] = 0;
// all is normal
// movement!
if (prevDuplicatePacket[axis] == true) {
// don't boost the packet after a duplicate, the FF 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
}
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
// determine if this step was a relatively large one, to use when evaluating next packet
// calculate acceleration, smooth and attenuate it
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
setpointAcceleration *= ffAttenuator;
if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){
bigStep[axis] = true;
} else {
bigStep[axis] = false;
}
// smooth deadband type suppression of FF jitter when sticks are at or returning to centre
// only when ff_averaging is 3 or more, for HD or cinematic flying
if (averagingCount > 2) {
const float rawSetpointCentred = absRawSetpoint / averagingCount;
if (rawSetpointCentred < 1.0f) {
setpointSpeed *= rawSetpointCentred;
setpointAcceleration *= rawSetpointCentred;
}
}
// smooth setpointSpeed but don't attenuate
setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
prevSetpointSpeed[axis] = setpointSpeed;
prevAcceleration[axis] = setpointAcceleration;
// all values afterwards are small numbers
prevRcCommandDelta[axis] = rcCommandDelta;
setpointAcceleration *= pidGetDT();
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
// calculate boost and prevent kick-back spike at max deflection
const float ffBoostFactor = pidGetFfBoostFactor();
float boostAmount = 0.0f;
if (ffBoostFactor != 0.0f) {
// calculate boost and prevent kick-back spike at max deflection
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier;
if (ffBoostFactor) {
if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100)));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF
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
}
// first order smoothing of boost to reduce jitter
const float ffSmoothFactor = pidGetFfSmoothFactor();
boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]);
prevBoostAmount[axis] = boostAmount;
// add boost to base feed forward
setpointDeltaImpl[axis] += boostAmount;
// first order smoothing of FF (second order boost filtering since boost filtered twice)
setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
prevDeltaImpl[axis] = setpointDeltaImpl[axis];
// apply averaging
if (type == FF_INTERPOLATE_ON) {
setpointDelta[axis] = setpointDeltaImpl[axis];

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

@ -205,7 +205,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dyn_idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_ON,
.ff_max_rate_limit = 100,
.ff_smooth_factor = 0,
.ff_smooth_factor = 37,
.ff_jitter_factor = 7,
.ff_boost = 15,
.dyn_lpf_curve_expo = 5,
.level_race_mode = false,
@ -266,6 +267,11 @@ float pidGetFfSmoothFactor()
{
return pidRuntime.ffSmoothFactor;
}
float pidGetFfJitterFactor()
{
return pidRuntime.ffJitterFactor;
}
#endif
void pidResetIterm(void)

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

@ -210,6 +210,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_smooth_factor; // Amount of smoothing for interpolated FF steps
uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
@ -385,6 +386,7 @@ typedef struct pidRuntime_s {
#ifdef USE_INTERPOLATED_SP
ffInterpolationType_t ffFromInterpolatedSetpoint;
float ffSmoothFactor;
float ffJitterFactor;
#endif
} pidRuntime_t;
@ -439,4 +441,5 @@ float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetFfSmoothFactor();
float pidGetFfJitterFactor();
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

1
src/main/flight/pid_init.c
View file

@ -392,6 +392,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
// set automatically according to boost amount, limit to 0.5 for auto
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f);
}
pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor;
interpolatedSpInit(pidProfile);
#endif