mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Mixer update: dynamic idle and throttle logging improvements
- all CLI parameters related to dynamic idle alone re-named with the `dyn_idle_` prefix - when linear throttle scaling is active, the user's set idle value is now correct whether dynamic idle is on or off. Previously, the idle value fell when dynamic idle was activated at the same time as linear throttle scaling. - enabling dynamic idle no longer causes a deadband at full throttle - the setpoint throttle value sent to Blackbox does not include the dynamic idle offset - the throttle value sent to the antigravity and dynamic lowpass code includes throttle scaling, but no other modifiers, to avoid false elevation of the apparent throttle position from dynamic idle and unnecessary transient changes in their filter cutoffs - Dynamic Idle now uses a modified PI controller during active rpm control phase - the D factor provides early detection of rapid falls in rpm, e.g. in hard chops. It is filtered heavily. Inadequate `dyn_idle_d_gain` may lead to a transient drop in rpm immediately after cutting throttle. Default is 50. - the P factor provides fast control over rpm during the active control phase. Too much `dyn_idle_p_gain` may cause oscillation in that phase. Note enough and a slow drop in rpm will be inadequately corrected. Default is 50. Needs to be higher with heavier larger props. - An integral element does most of the work. It prevents enduring offsets from the set rpm. The I gain is high when increasing responding to low rpm, and slow to release. The slow release makes a huge difference and avoids I oscillation. Not enough `dyn_idle_i_gain` and there may be wobble in rpm during the control phase, or the idle value may rise too slowly; too much may cause wobble. Default is 50. Needs to be higher with heavier larger props. - The DYN_IDLE debug shows idle P, I and D in debugs 0, 1 and 2. minRps stays in debug 3. - Interactions between throttle and thrust linear, dynamic idle, throttle scaling and throttle boost have been checked and work as they should.
This commit is contained in:
ctzsnooze
parent
8ee317f815
commit
8050ecd1e7
12 changed files with 116 additions and 78 deletions
|
|
@ -216,22 +216,28 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
|
|||
}
|
||||
} else {
|
||||
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
|
||||
#ifdef USE_DYN_IDLE
|
||||
if (mixerRuntime.idleMinMotorRps > 0.0f) {
|
||||
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.idleMaxIncrease : 0.04f;
|
||||
const float minRps = rpmMinMotorFrequency();
|
||||
const float targetRpsChangeRate = (mixerRuntime.idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed;
|
||||
const float error = targetRpsChangeRate - (minRps - mixerRuntime.oldMinRps) * pidGetPidFrequency();
|
||||
const float pidSum = constrainf(mixerRuntime.idleP * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
|
||||
motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * pidGetDT(), 0.0f, maxIncrease);
|
||||
mixerRuntime.oldMinRps = minRps;
|
||||
throttle += mixerRuntime.idleThrottleOffset;
|
||||
currentThrottleInputRange = PWM_RANGE;
|
||||
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 0, motorRangeMinIncrease * 1000);
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpsChangeRate);
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 2, error);
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 3, minRps);
|
||||
} else {
|
||||
#ifdef USE_DYN_IDLE
|
||||
if (mixerRuntime.dynIdleMinRps > 0.0f) {
|
||||
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.04f;
|
||||
float minRps = rpmMinMotorFrequency();
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 3, (minRps * 10));
|
||||
float rpsError = mixerRuntime.dynIdleMinRps - minRps;
|
||||
// PT1 type lowpass delay and smoothing for D
|
||||
minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps);
|
||||
float dynIdleD = (mixerRuntime.prevMinRps - minRps) * mixerRuntime.dynIdleDGain;
|
||||
mixerRuntime.prevMinRps = minRps;
|
||||
float dynIdleP = rpsError * mixerRuntime.dynIdlePGain;
|
||||
rpsError = MAX(-0.1f, rpsError); //I rises fast, falls slowly
|
||||
mixerRuntime.dynIdleI += rpsError * mixerRuntime.dynIdleIGain;
|
||||
mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease);
|
||||
motorRangeMinIncrease = constrainf((dynIdleP + mixerRuntime.dynIdleI + dynIdleD), 0.0f, maxIncrease);
|
||||
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 0, (MAX(-1000.0f, dynIdleP * 10000)));
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 1, (mixerRuntime.dynIdleI * 10000));
|
||||
DEBUG_SET(DEBUG_DYN_IDLE, 2, (dynIdleD * 10000));
|
||||
} else {
|
||||
motorRangeMinIncrease = 0;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -252,7 +258,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
|
|||
motorRangeMax = mixerRuntime.motorOutputHigh;
|
||||
#endif
|
||||
|
||||
currentThrottleInputRange = PWM_RANGE;
|
||||
motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
|
||||
motorOutputMin = motorRangeMin;
|
||||
motorOutputRange = motorRangeMax - motorRangeMin;
|
||||
|
|
@ -467,25 +472,39 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
throttle = applyThrottleLimit(throttle);
|
||||
}
|
||||
|
||||
const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
|
||||
// use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
|
||||
pidUpdateAntiGravityThrottleFilter(throttle);
|
||||
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
// 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
|
||||
// When airmode is active the throttle setting doesn't impact recovery authority.
|
||||
if (yawSpinDetected && !airmodeEnabled) {
|
||||
throttle = 0.5f; //
|
||||
}
|
||||
#endif // USE_YAW_SPIN_RECOVERY
|
||||
#ifdef USE_DYN_LPF
|
||||
// keep the changes to dynamic lowpass clean, without unnecessary dynamic changes
|
||||
updateDynLpfCutoffs(currentTimeUs, throttle);
|
||||
#endif
|
||||
|
||||
#ifdef USE_LAUNCH_CONTROL
|
||||
// While launch control is active keep the throttle at minimum.
|
||||
// Once the pilot triggers the launch throttle control will be reactivated.
|
||||
if (launchControlActive) {
|
||||
throttle = 0.0f;
|
||||
// apply throttle boost when throttle moves quickly
|
||||
#if defined(USE_THROTTLE_BOOST)
|
||||
if (throttleBoost > 0.0f) {
|
||||
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
|
||||
throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
|
||||
}
|
||||
#endif
|
||||
|
||||
// send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode
|
||||
mixerThrottle = throttle;
|
||||
|
||||
#ifdef USE_DYN_IDLE
|
||||
// Apply digital idle throttle offset when stick is at zero after all other adjustments are complete
|
||||
if (mixerRuntime.dynIdleMinRps > 0.0f) {
|
||||
throttle = MAX(throttle, mixerRuntime.idleThrottleOffset);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_THRUST_LINEARIZATION
|
||||
// reduce throttle to offset additional motor output
|
||||
throttle = pidCompensateThrustLinearization(throttle);
|
||||
#endif
|
||||
|
||||
// Find roll/pitch/yaw desired output
|
||||
// ??? Where is the optimal location for this code?
|
||||
float motorMix[MAX_SUPPORTED_MOTORS];
|
||||
float motorMixMax = 0, motorMixMin = 0;
|
||||
for (int i = 0; i < mixerRuntime.motorCount; i++) {
|
||||
|
|
@ -503,21 +522,22 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
motorMix[i] = mix;
|
||||
}
|
||||
|
||||
pidUpdateAntiGravityThrottleFilter(throttle);
|
||||
// The following fixed throttle values will not be shown in the blackbox log
|
||||
// ?? Should they be influenced by airmode? If not, should go after the apply airmode code.
|
||||
const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
// 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
|
||||
// When airmode is active the throttle setting doesn't impact recovery authority.
|
||||
if (yawSpinDetected && !airmodeEnabled) {
|
||||
throttle = 0.5f;
|
||||
}
|
||||
#endif // USE_YAW_SPIN_RECOVERY
|
||||
|
||||
#ifdef USE_DYN_LPF
|
||||
updateDynLpfCutoffs(currentTimeUs, throttle);
|
||||
#endif
|
||||
|
||||
#ifdef USE_THRUST_LINEARIZATION
|
||||
// reestablish old throttle stick feel by counter compensating thrust linearization
|
||||
throttle = pidCompensateThrustLinearization(throttle);
|
||||
#endif
|
||||
|
||||
#if defined(USE_THROTTLE_BOOST)
|
||||
if (throttleBoost > 0.0f) {
|
||||
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
|
||||
throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
|
||||
#ifdef USE_LAUNCH_CONTROL
|
||||
// While launch control is active keep the throttle at minimum.
|
||||
// Once the pilot triggers the launch throttle control will be reactivated.
|
||||
if (launchControlActive) {
|
||||
throttle = 0.0f;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -534,8 +554,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
throttle += pidGetAirmodeThrottleOffset();
|
||||
float airmodeThrottleChange = 0;
|
||||
#endif
|
||||
mixerThrottle = throttle;
|
||||
|
||||
// apply airmode
|
||||
motorMixRange = motorMixMax - motorMixMin;
|
||||
if (motorMixRange > 1.0f) {
|
||||
for (int i = 0; i < mixerRuntime.motorCount; i++) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue