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

Merge pull request #8842 from ctzsnooze/ff_spike_suppression 

Small modification to ff boost to improve spike suppression
This commit is contained in:
Michael Keller 2019-09-09 00:56:19 +12:00 committed by GitHub
parent f331cb3dd1 633f404855
commit 171535aaf5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View File

@ -1343,7 +1343,7 @@ static bool blackboxWriteSysinfo(void)
currentPidProfile->pid[PID_YAW].F);
#ifdef USE_INTERPOLATED_SP
BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d", currentPidProfile->ff_interpolate_sp);
BLACKBOX_PRINT_HEADER_LINE("ff_jerk_limit", "%d", currentPidProfile->ff_jerk_limit);
BLACKBOX_PRINT_HEADER_LINE("ff_spike_limit", "%d", currentPidProfile->ff_spike_limit);
BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->ff_max_rate_limit);
#endif
BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->ff_boost);

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

@ -1080,7 +1080,7 @@ const clivalue_t valueTable[] = {
#endif
#ifdef USE_INTERPOLATED_SP
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_jerk_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jerk_limit) },
{ "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) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },

9
src/main/flight/interpolated_setpoint.c
View File

@ -60,16 +60,15 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
const float setpointJerk = setpointAcceleration - prevSetpointAcceleration[axis];
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
const float ffBoostFactor = pidGetFfBoostFactor();
float clip = 1.0f;
float boostAmount = 0.0f;
if (ffBoostFactor != 0.0f) {
if (pidGetJerkLimitInverse()) {
clip = 1 / (1 + fabsf(setpointJerk * pidGetJerkLimitInverse()));
if (pidGetSpikeLimitInverse()) {
clip = 1 / (1 + fabsf(setpointAcceleration * pidGetSpikeLimitInverse()));
clip *= clip;
}
@ -86,7 +85,7 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
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, setpointJerk * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, clip * 100);
}
setpointDeltaImpl[axis] += boostAmount * clip;
if (type == FF_INTERPOLATE_ON) {

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

@ -213,7 +213,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_pid_limit = 200,
.idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_AVG,
.ff_jerk_limit = 25,
.ff_spike_limit = 40,
.ff_max_rate_limit = 100,
.ff_boost = 15,
);
@ -314,11 +314,11 @@ 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 ffJerkLimitInverse;
static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse;
float pidGetJerkLimitInverse()
float pidGetSpikeLimitInverse()
{
return ffJerkLimitInverse;
return ffSpikeLimitInverse;
}
@ -466,7 +466,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
ffJerkLimitInverse = pidProfile->ff_jerk_limit ? 1.0f / ((float)pidProfile->ff_jerk_limit / 10.0f) : 0.0f;
ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f;
}
#ifdef USE_RC_SMOOTHING_FILTER

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

@ -183,7 +183,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_jerk_limit; // FF stick extrapolation lookahead period in ms
uint8_t ff_spike_limit; // FF stick extrapolation lookahead period in ms
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -261,4 +261,4 @@ float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetJerkLimitInverse();
float pidGetSpikeLimitInverse();