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

First attempt low throttle airmode oscillation fix

Browse source 
raise dc offset immediately

Add airmode noise fix

no filter when inactive and slightly more throttle

hpf throttle to avoid large moves increasing throttle persistently

fix ws

fix ununsed variable and default to off

include airmode throttle offset in loggingThrottle
This commit is contained in:
Thorsten Laux 2019-01-26 12:28:51 +01:00 • committed by mikeller
parent 966971c750
commit e16ef1db4f
5 changed files with 66 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -117,6 +117,10 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER
#ifdef USE_AIRMODE_LPF
static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
#endif
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
#define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
@ -196,6 +200,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.d_min_advance = 20,
.motor_output_limit = 100,
.auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
.transient_throttle_limit = 0,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150;
@ -287,6 +292,11 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_AIRMODE_LPF
static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1;
static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
#endif
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile)
@ -415,6 +425,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT));
}
#endif
#if defined(USE_AIRMODE_LPF)
if (pidProfile->transient_throttle_limit) {
pt1FilterInit(&airmodeThrottleLpf1, pt1FilterGain(7.0f, dT));
pt1FilterInit(&airmodeThrottleLpf2, pt1FilterGain(20.0f, dT));
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
@ -674,6 +690,9 @@ void pidInitConfig(const pidProfile_t *pidProfile)
dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidFrequency / (100 * D_MIN_LOWPASS_HZ);
// lowpass included inversely in gain since stronger lowpass decreases peak effect
#endif
#if defined(USE_AIRMODE_LPF)
airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -1117,6 +1136,31 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
}
#endif
#ifdef USE_AIRMODE_LPF
void pidUpdateAirmodeLpf(float currentOffset)
{
if (airmodeThrottleOffsetLimit == 0.0f) {
return;
}
float offsetHpf = currentOffset * 2.5f;
offsetHpf = offsetHpf - pt1FilterApply(&airmodeThrottleLpf2, offsetHpf);
// During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
pt1FilterApply(&airmodeThrottleLpf1, offsetHpf);
// Bring offset up immediately so the filter only applies to the decline
if (currentOffset * airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > airmodeThrottleLpf1.state) {
airmodeThrottleLpf1.state = currentOffset;
}
airmodeThrottleLpf1.state = constrainf(airmodeThrottleLpf1.state, -airmodeThrottleOffsetLimit, airmodeThrottleOffsetLimit);
}
float pidGetAirmodeThrottleOffset()
{
return airmodeThrottleLpf1.state;
}
#endif
#ifdef USE_LAUNCH_CONTROL
#define LAUNCH_CONTROL_MAX_RATE 100.0f
#define LAUNCH_CONTROL_MIN_RATE 5.0f