mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 20:10:18 +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
|
|
@ -1476,7 +1476,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
#endif
|
||||
|
||||
#if defined(USE_DYN_IDLE)
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamic_idle_min_rpm", "%d", currentPidProfile->idle_min_rpm);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamic_idle_min_rpm", "%d", currentPidProfile->dyn_idle_min_rpm);
|
||||
#endif
|
||||
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -1142,9 +1142,11 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_THRUST_LINEARIZATION
|
||||
{ "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) },
|
||||
#endif
|
||||
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
|
||||
#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_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
|
||||
|
|
@ -1153,11 +1155,11 @@ const clivalue_t valueTable[] = {
|
|||
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
|
||||
|
||||
#ifdef USE_DYN_IDLE
|
||||
{ "idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_min_rpm) },
|
||||
{ "idle_adjustment_speed", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_adjustment_speed) },
|
||||
{ "idle_p", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_p) },
|
||||
{ "idle_pid_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) },
|
||||
{ "idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) },
|
||||
{ "dyn_idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_min_rpm) },
|
||||
{ "dyn_idle_p_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_p_gain) },
|
||||
{ "dyn_idle_i_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_i_gain) },
|
||||
{ "dyn_idle_d_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_d_gain) },
|
||||
{ "dyn_idle_max_increase", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_max_increase) },
|
||||
#endif
|
||||
{ "level_race_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, level_race_mode) },
|
||||
|
||||
|
|
|
|||
|
|
@ -533,7 +533,7 @@ static void validateAndFixConfig(void)
|
|||
#if defined(USE_DYN_IDLE)
|
||||
if (!isRpmFilterEnabled()) {
|
||||
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
|
||||
pidProfilesMutable(i)->idle_min_rpm = 0;
|
||||
pidProfilesMutable(i)->dyn_idle_min_rpm = 0;
|
||||
}
|
||||
}
|
||||
#endif // USE_DYN_IDLE
|
||||
|
|
|
|||
|
|
@ -330,6 +330,6 @@ bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
|
|||
|
||||
float getDigitalIdleOffset(const motorConfig_t *motorConfig)
|
||||
{
|
||||
return CONVERT_PARAMETER_TO_PERCENT(motorConfig->digitalIdleOffsetValue * 0.01f);
|
||||
return CONVERT_PARAMETER_TO_PERCENT(motorConfig->digitalIdleOffsetValue * 0.01f);
|
||||
}
|
||||
#endif // USE_MOTOR
|
||||
|
|
|
|||
|
|
@ -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++) {
|
||||
|
|
|
|||
|
|
@ -289,19 +289,22 @@ void initEscEndpoints(void)
|
|||
}
|
||||
|
||||
motorInitEndpoints(motorConfig(), motorOutputLimit, &mixerRuntime.motorOutputLow, &mixerRuntime.motorOutputHigh, &mixerRuntime.disarmMotorOutput, &mixerRuntime.deadbandMotor3dHigh, &mixerRuntime.deadbandMotor3dLow);
|
||||
if (!mixerRuntime.feature3dEnabled && currentPidProfile->idle_min_rpm) {
|
||||
if (!mixerRuntime.feature3dEnabled && currentPidProfile->dyn_idle_min_rpm) {
|
||||
mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize pidProfile related mixer settings
|
||||
|
||||
void mixerInitProfile(void)
|
||||
{
|
||||
#ifdef USE_DYN_IDLE
|
||||
mixerRuntime.idleMinMotorRps = currentPidProfile->idle_min_rpm * 100.0f / 60.0f;
|
||||
mixerRuntime.idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f;
|
||||
mixerRuntime.idleP = currentPidProfile->idle_p * 0.0001f;
|
||||
mixerRuntime.oldMinRps = 0;
|
||||
mixerRuntime.dynIdleMinRps = currentPidProfile->dyn_idle_min_rpm * 100.0f / 60.0f;
|
||||
mixerRuntime.dynIdlePGain = currentPidProfile->dyn_idle_p_gain * 0.00015f;
|
||||
mixerRuntime.dynIdleIGain = currentPidProfile->dyn_idle_i_gain * 0.01f * pidGetDT();
|
||||
mixerRuntime.dynIdleDGain = currentPidProfile->dyn_idle_d_gain * 0.0000003f * pidGetPidFrequency();
|
||||
mixerRuntime.dynIdleMaxIncrease = currentPidProfile->dyn_idle_max_increase * 0.001f;
|
||||
mixerRuntime.minRpsDelayK = 800 * pidGetDT() / 20.0f; //approx 20ms D delay, arbitrarily suits many motors
|
||||
#endif
|
||||
|
||||
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
|
||||
|
|
@ -410,7 +413,9 @@ void mixerInit(mixerMode_e mixerMode)
|
|||
#endif
|
||||
|
||||
#ifdef USE_DYN_IDLE
|
||||
mixerRuntime.idleThrottleOffset = getDigitalIdleOffset(motorConfig()) * PWM_RANGE;
|
||||
mixerRuntime.idleThrottleOffset = getDigitalIdleOffset(motorConfig());
|
||||
mixerRuntime.dynIdleI = 0.0f;
|
||||
mixerRuntime.prevMinRps = 0.0f;
|
||||
#endif
|
||||
|
||||
mixerConfigureOutput();
|
||||
|
|
|
|||
|
|
@ -38,11 +38,15 @@ typedef struct mixerRuntime_s {
|
|||
float deadbandMotor3dHigh;
|
||||
float deadbandMotor3dLow;
|
||||
#ifdef USE_DYN_IDLE
|
||||
float idleMaxIncrease;
|
||||
float dynIdleMaxIncrease;
|
||||
float idleThrottleOffset;
|
||||
float idleMinMotorRps;
|
||||
float idleP;
|
||||
float oldMinRps;
|
||||
float dynIdleMinRps;
|
||||
float dynIdlePGain;
|
||||
float prevMinRps;
|
||||
float dynIdleIGain;
|
||||
float dynIdleDGain;
|
||||
float dynIdleI;
|
||||
float minRpsDelayK;
|
||||
#endif
|
||||
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
|
||||
float vbatSagCompensationFactor;
|
||||
|
|
|
|||
|
|
@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
|||
|
||||
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
|
||||
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 1);
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2);
|
||||
|
||||
void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
|
|
@ -197,11 +197,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
|
||||
.transient_throttle_limit = 0,
|
||||
.profileName = { 0 },
|
||||
.idle_min_rpm = 0,
|
||||
.idle_adjustment_speed = 50,
|
||||
.idle_p = 50,
|
||||
.idle_pid_limit = 200,
|
||||
.idle_max_increase = 150,
|
||||
.dyn_idle_min_rpm = 0,
|
||||
.dyn_idle_p_gain = 50,
|
||||
.dyn_idle_i_gain = 50,
|
||||
.dyn_idle_d_gain = 50,
|
||||
.dyn_idle_max_increase = 150,
|
||||
.ff_interpolate_sp = FF_INTERPOLATE_ON,
|
||||
.ff_max_rate_limit = 100,
|
||||
.ff_smooth_factor = 37,
|
||||
|
|
|
|||
|
|
@ -188,11 +188,11 @@ typedef struct pidProfile_s {
|
|||
uint8_t ff_boost; // amount of high-pass filtered FF to add to FF, 100 means 100% added
|
||||
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
|
||||
|
||||
uint8_t idle_min_rpm; // minimum motor speed enforced by integrating p controller
|
||||
uint8_t idle_adjustment_speed; // how quickly the integrating p controller tries to correct
|
||||
uint8_t idle_p; // kP
|
||||
uint8_t idle_pid_limit; // max P
|
||||
uint8_t idle_max_increase; // max integrated correction
|
||||
uint8_t dyn_idle_min_rpm; // minimum motor speed enforced by the dynamic idle controller
|
||||
uint8_t dyn_idle_p_gain; // P gain during active control of rpm
|
||||
uint8_t dyn_idle_i_gain; // I gain during active control of rpm
|
||||
uint8_t dyn_idle_d_gain; // D gain for corrections around rapid changes in rpm
|
||||
uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control
|
||||
|
||||
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
|
||||
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
|
||||
|
|
@ -386,10 +386,12 @@ bool pidOsdAntiGravityActive(void);
|
|||
bool pidOsdAntiGravityMode(void);
|
||||
void pidSetAntiGravityState(bool newState);
|
||||
bool pidAntiGravityEnabled(void);
|
||||
|
||||
#ifdef USE_THRUST_LINEARIZATION
|
||||
float pidApplyThrustLinearization(float motorValue);
|
||||
float pidCompensateThrustLinearization(float throttle);
|
||||
#endif
|
||||
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
void pidUpdateAirmodeLpf(float currentOffset);
|
||||
float pidGetAirmodeThrottleOffset();
|
||||
|
|
|
|||
|
|
@ -170,6 +170,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
#if defined(USE_THROTTLE_BOOST)
|
||||
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, pidRuntime.dT));
|
||||
#endif
|
||||
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
if (pidRuntime.itermRelax) {
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
|
|
@ -177,6 +178,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
if (pidRuntime.itermRelax) {
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
|
|
@ -184,8 +186,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_D_MIN)
|
||||
|
||||
#if defined(USE_D_MIN)
|
||||
// Initialize the filters for all axis even if the d_min[axis] value is 0
|
||||
// Otherwise if the pidProfile->d_min_xxx parameters are ever added to
|
||||
// in-flight adjustments and transition from 0 to > 0 in flight the feature
|
||||
|
|
@ -195,6 +197,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT));
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_AIRMODE_LPF)
|
||||
if (pidProfile->transient_throttle_limit) {
|
||||
pt1FilterInit(&pidRuntime.airmodeThrottleLpf1, pt1FilterGain(7.0f, pidRuntime.dT));
|
||||
|
|
@ -274,7 +277,6 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
{
|
||||
pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
|
||||
}
|
||||
|
||||
pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
||||
pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
|
||||
pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
|
||||
|
|
@ -378,6 +380,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
|
||||
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powerf(pidRuntime.thrustLinearization, 2);
|
||||
#endif
|
||||
|
||||
#if defined(USE_D_MIN)
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
const uint8_t dMin = pidProfile->d_min[axis];
|
||||
|
|
@ -391,9 +394,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
pidRuntime.dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidRuntime.pidFrequency / (100 * D_MIN_LOWPASS_HZ);
|
||||
// lowpass included inversely in gain since stronger lowpass decreases peak effect
|
||||
#endif
|
||||
|
||||
#if defined(USE_AIRMODE_LPF)
|
||||
pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
|
||||
#endif
|
||||
|
||||
#ifdef USE_INTERPOLATED_SP
|
||||
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
|
||||
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
|
||||
|
|
|
|||
|
|
@ -173,6 +173,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
|
|||
if (motor < 4) {
|
||||
DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
|
||||
}
|
||||
motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
|
||||
}
|
||||
|
||||
for (int i = 0; i < filterUpdatesPerIteration; i++) {
|
||||
|
|
@ -202,7 +203,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
|
|||
if (++currentMotor == getMotorCount()) {
|
||||
currentMotor = 0;
|
||||
}
|
||||
motorFrequency[currentMotor] = erpmToHz * filteredMotorErpm[currentMotor];
|
||||
minMotorFrequency = 0.0f;
|
||||
}
|
||||
currentFilter = &filters[currentFilterNumber];
|
||||
|
|
|
|||
|
|
@ -1844,7 +1844,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU8(dst, currentPidProfile->motor_output_limit);
|
||||
sbufWriteU8(dst, currentPidProfile->auto_profile_cell_count);
|
||||
#if defined(USE_DYN_IDLE)
|
||||
sbufWriteU8(dst, currentPidProfile->idle_min_rpm);
|
||||
sbufWriteU8(dst, currentPidProfile->dyn_idle_min_rpm);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
|
|
@ -2714,7 +2714,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
currentPidProfile->motor_output_limit = sbufReadU8(src);
|
||||
currentPidProfile->auto_profile_cell_count = sbufReadU8(src);
|
||||
#if defined(USE_DYN_IDLE)
|
||||
currentPidProfile->idle_min_rpm = sbufReadU8(src);
|
||||
currentPidProfile->dyn_idle_min_rpm = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue