mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Update D_CUT to D_MIN, add setpoint
Directly and easily set the minimum value for D on pitch and roll. - Boost back to the primary D setting is generated from gyro or setpoint inputs. - Setpoint input is stick derived, faster by 10ms approx, and does not respond to propwash. - Gyro input is motor derived and slower, but responds to propwash - timing value sets balance between gyro (100) and setpoint (0) boost factors - gain value sets overall sensitivity - default D mins are 20 roll 22 pitch - default D is 35, 38; if undefined then normal 30, 32 values TUNING - D value to flip overshoot control - D_min to noise, lower values mean cooler motors but perhaps more propwash. - Advance, higher values bring the boost in earlier, and stronger overall, useful for very high flip rates, but dampen stick responsiveness slightly. - Gain value adjust against logs, checking maximal boost with flips and some rise with propwash, should be edging to get up from the min value in normal flight.
This commit is contained in:
ctzsnooze
parent
4f7fa25b06
commit
b70d34f9db
7 changed files with 105 additions and 90 deletions
|
|
@ -81,5 +81,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"RX_SPEKTRUM_SPI",
|
||||
"DSHOT_RPM_TELEMETRY",
|
||||
"RPM_FILTER",
|
||||
"D_CUT",
|
||||
"D_MIN",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ typedef enum {
|
|||
DEBUG_RX_SPEKTRUM_SPI,
|
||||
DEBUG_DSHOT_RPM_TELEMETRY,
|
||||
DEBUG_RPM_FILTER,
|
||||
DEBUG_D_CUT,
|
||||
DEBUG_D_MIN,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -1002,19 +1002,20 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
#ifdef USE_INTEGRATED_YAW_CONTROL
|
||||
{ "use_integrated_yaw", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) },
|
||||
{ "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },
|
||||
{ "use_integrated_yaw", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) },
|
||||
{ "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },
|
||||
#endif
|
||||
|
||||
#ifdef USE_D_CUT
|
||||
{ "dterm_cut_percent", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_percent) },
|
||||
{ "dterm_cut_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_gain) },
|
||||
{ "dterm_cut_range_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_range_hz) },
|
||||
{ "dterm_cut_lowpass_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_lowpass_hz) },
|
||||
#ifdef USE_D_MIN
|
||||
{ "d_min_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_roll) },
|
||||
{ "d_min_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_pitch) },
|
||||
{ "d_min_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_yaw) },
|
||||
{ "d_min_boost_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_gain) },
|
||||
{ "d_min_advance", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_advance) },
|
||||
#endif
|
||||
|
||||
{ "motor_output_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { MOTOR_OUTPUT_LIMIT_PERCENT_MIN, MOTOR_OUTPUT_LIMIT_PERCENT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, motor_output_limit) },
|
||||
{ "auto_profile_cell_count", VAR_INT8 | PROFILE_VALUE, .config.minmax = { AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT }, PG_PID_PROFILE, offsetof(pidProfile_t, auto_profile_cell_count) },
|
||||
{ "motor_output_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { MOTOR_OUTPUT_LIMIT_PERCENT_MIN, MOTOR_OUTPUT_LIMIT_PERCENT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, motor_output_limit) },
|
||||
{ "auto_profile_cell_count", VAR_INT8 | PROFILE_VALUE, .config.minmax = { AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT }, PG_PID_PROFILE, offsetof(pidProfile_t, auto_profile_cell_count) },
|
||||
|
||||
#ifdef USE_LAUNCH_CONTROL
|
||||
{ "launch_control_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
|
||||
|
|
|
|||
|
|
@ -560,8 +560,10 @@ static uint16_t cmsx_dterm_lowpass_hz;
|
|||
static uint16_t cmsx_dterm_lowpass2_hz;
|
||||
static uint16_t cmsx_dterm_notch_hz;
|
||||
static uint16_t cmsx_dterm_notch_cutoff;
|
||||
#ifdef USE_D_CUT
|
||||
static uint8_t cmsx_dterm_cut_percent;
|
||||
#ifdef USE_D_MIN
|
||||
static uint8_t cmsx_d_min_roll;
|
||||
static uint8_t cmsx_d_min_pitch;
|
||||
static uint8_t cmsx_d_min_yaw;
|
||||
#endif
|
||||
static uint16_t cmsx_yaw_lowpass_hz;
|
||||
|
||||
|
|
@ -573,8 +575,10 @@ static long cmsx_FilterPerProfileRead(void)
|
|||
cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz;
|
||||
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
|
||||
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
|
||||
#ifdef USE_D_CUT
|
||||
cmsx_dterm_cut_percent = pidProfile->dterm_cut_percent;
|
||||
#ifdef USE_D_MIN
|
||||
cmsx_d_min_roll = pidProfile->d_min_roll;
|
||||
cmsx_d_min_pitch = pidProfile->d_min_pitch;
|
||||
cmsx_d_min_yaw = pidProfile->d_min_yaw;
|
||||
#endif
|
||||
cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz;
|
||||
|
||||
|
|
@ -591,8 +595,10 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
|
|||
pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz;
|
||||
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
|
||||
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
|
||||
#ifdef USE_D_CUT
|
||||
pidProfile->dterm_cut_percent = cmsx_dterm_cut_percent;
|
||||
#ifdef USE_D_MIN
|
||||
pidProfile->d_min_roll = cmsx_d_min_roll;
|
||||
pidProfile->d_min_pitch = cmsx_d_min_pitch;
|
||||
pidProfile->d_min_yaw = cmsx_d_min_yaw;
|
||||
#endif
|
||||
pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz;
|
||||
|
||||
|
|
@ -607,8 +613,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] =
|
|||
{ "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, 500, 1 }, 0 },
|
||||
{ "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 },
|
||||
{ "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 },
|
||||
#ifdef USE_D_CUT
|
||||
{ "DTERM CUT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_dterm_cut_percent, 0, 100, 1 }, 0 },
|
||||
#ifdef USE_D_MIN
|
||||
{ "D_MIN_ROLL", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_roll, 0, 100, 1 }, 0 },
|
||||
{ "D_MIN_PITCH", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_pitch, 0, 100, 1 }, 0 },
|
||||
{ "D_MIN_YAW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_yaw, 0, 100, 1 }, 0 },
|
||||
#endif
|
||||
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 },
|
||||
{ "BACK", OME_Back, NULL, NULL, 0 },
|
||||
|
|
|
|||
|
|
@ -92,8 +92,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
|
|||
#else
|
||||
#define PID_PROCESS_DENOM_DEFAULT 2
|
||||
#endif
|
||||
#if defined(USE_D_CUT)
|
||||
#define D_CUT_GAIN_FACTOR 0.00005f
|
||||
#if defined(USE_D_MIN)
|
||||
#define D_MIN_GAIN_FACTOR 0.00005f
|
||||
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
|
||||
#define D_MIN_RANGE_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies
|
||||
#define D_MIN_LOWPASS_HZ 10 // PT1 lowpass cutoff to smooth the boost effect
|
||||
#endif
|
||||
|
||||
#ifdef USE_RUNAWAY_TAKEOFF
|
||||
|
|
@ -126,8 +129,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
{
|
||||
RESET_CONFIG(pidProfile_t, pidProfile,
|
||||
.pid = {
|
||||
[PID_ROLL] = { 46, 65, 40, 60 },
|
||||
[PID_PITCH] = { 50, 75, 44, 60 },
|
||||
[PID_ROLL] = { 46, 65, 35, 60 },
|
||||
[PID_PITCH] = { 50, 75, 38, 60 },
|
||||
[PID_YAW] = { 45, 100, 0, 100 },
|
||||
[PID_LEVEL] = { 50, 50, 75, 0 },
|
||||
[PID_MAG] = { 40, 0, 0, 0 },
|
||||
|
|
@ -188,10 +191,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.use_integrated_yaw = false,
|
||||
.integrated_yaw_relax = 200,
|
||||
.thrustLinearization = 0,
|
||||
.dterm_cut_percent = 65,
|
||||
.dterm_cut_gain = 15,
|
||||
.dterm_cut_range_hz = 40,
|
||||
.dterm_cut_lowpass_hz = 7,
|
||||
.d_min_roll = 20,
|
||||
.d_min_pitch = 22,
|
||||
.d_min_yaw = 0,
|
||||
.d_min_gain = 20,
|
||||
.d_min_advance = 20,
|
||||
.motor_output_limit = 100,
|
||||
.auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
|
||||
);
|
||||
|
|
@ -201,7 +205,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->dterm_filter_type = FILTER_BIQUAD;
|
||||
pidProfile->dterm_filter2_type = FILTER_BIQUAD;
|
||||
#endif
|
||||
#ifndef USE_D_CUT
|
||||
#ifndef USE_D_MIN
|
||||
pidProfile->pid[PID_ROLL].D = 30;
|
||||
pidProfile->pid[PID_PITCH].D = 32;
|
||||
#endif
|
||||
|
|
@ -272,11 +276,10 @@ static FAST_RAM_ZERO_INIT float acCutoff;
|
|||
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
|
||||
#if defined(USE_D_CUT)
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutRangeApplyFn;
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t dtermCutRange[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutLowpassApplyFn;
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t dtermCutLowpass[XYZ_AXIS_COUNT];
|
||||
#if defined(USE_D_MIN)
|
||||
static FAST_RAM_ZERO_INIT uint8_t dMin[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
|
|
@ -404,18 +407,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_D_CUT)
|
||||
if (pidProfile->dterm_cut_percent == 0) {
|
||||
dtermCutRangeApplyFn = nullFilterApply;
|
||||
dtermCutLowpassApplyFn = nullFilterApply;
|
||||
} else {
|
||||
dtermCutRangeApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
dtermCutLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
biquadFilterInitLPF(&dtermCutRange[axis], pidProfile->dterm_cut_range_hz, targetPidLooptime);
|
||||
pt1FilterInit(&dtermCutLowpass[axis], pt1FilterGain(pidProfile->dterm_cut_lowpass_hz, dT));
|
||||
}
|
||||
}
|
||||
#if defined(USE_D_MIN)
|
||||
dMin[FD_ROLL] = pidProfile->d_min_roll;
|
||||
dMin[FD_PITCH] = pidProfile->d_min_pitch;
|
||||
dMin[FD_YAW] = pidProfile->d_min_yaw;
|
||||
|
||||
// Initialize the filters for all axis even if the dMin[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
|
||||
// won't work because the filter wasn't initialized.
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
biquadFilterInitLPF(&dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
|
||||
pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT));
|
||||
}
|
||||
#endif
|
||||
|
||||
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
|
||||
|
|
@ -539,14 +543,11 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
|
|||
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
|
||||
#endif
|
||||
|
||||
#ifdef USE_D_CUT
|
||||
static FAST_RAM_ZERO_INIT float dtermCutPercent;
|
||||
static FAST_RAM_ZERO_INIT float dtermCutPercentInv;
|
||||
static FAST_RAM_ZERO_INIT float dtermCutGain;
|
||||
static FAST_RAM_ZERO_INIT float dtermCutRangeHz;
|
||||
static FAST_RAM_ZERO_INIT float dtermCutLowpassHz;
|
||||
#ifdef USE_D_MIN
|
||||
static FAST_RAM_ZERO_INIT float dMinPercent[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT float dMinGyroGain;
|
||||
static FAST_RAM_ZERO_INIT float dMinSetpointGain;
|
||||
#endif
|
||||
static FAST_RAM_ZERO_INIT float dtermCutFactor;
|
||||
|
||||
void pidInitConfig(const pidProfile_t *pidProfile)
|
||||
{
|
||||
|
|
@ -666,15 +667,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization);
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_D_CUT)
|
||||
dtermCutPercent = pidProfile->dterm_cut_percent / 100.0f;
|
||||
dtermCutPercentInv = 1.0f - dtermCutPercent;
|
||||
dtermCutRangeHz = pidProfile->dterm_cut_range_hz;
|
||||
dtermCutLowpassHz = pidProfile->dterm_cut_lowpass_hz;
|
||||
dtermCutGain = pidProfile->dterm_cut_gain * dtermCutPercent * D_CUT_GAIN_FACTOR / dtermCutLowpassHz;
|
||||
#if defined(USE_D_MIN)
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
if ((dMin[axis] > 0) && (dMin[axis] < pidProfile->pid[axis].D)) {
|
||||
dMinPercent[axis] = dMin[axis] / (float)(pidProfile->pid[axis].D);
|
||||
} else {
|
||||
dMinPercent[axis] = 0;
|
||||
}
|
||||
}
|
||||
dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
|
||||
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
|
||||
dtermCutFactor = 1.0f;
|
||||
}
|
||||
|
||||
void pidInit(const pidProfile_t *pidProfile)
|
||||
|
|
@ -1313,6 +1317,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
#endif
|
||||
pidData[axis].I = constrainf(iterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
|
||||
|
||||
// -----calculate pidSetpointDelta
|
||||
float pidSetpointDelta = 0;
|
||||
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
// -----calculate D component
|
||||
// disable D if launch control is active
|
||||
if ((pidCoefficient[axis].Kd > 0) && !launchControlActive){
|
||||
|
|
@ -1331,44 +1344,37 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_D_CUT)
|
||||
if (dtermCutPercent){
|
||||
dtermCutFactor = dtermCutRangeApplyFn((filter_t *) &dtermCutRange[axis], delta);
|
||||
dtermCutFactor = fabsf(dtermCutFactor) * dtermCutGain;
|
||||
dtermCutFactor = dtermCutLowpassApplyFn((filter_t *) &dtermCutLowpass[axis], dtermCutFactor);
|
||||
dtermCutFactor = MIN(dtermCutFactor, 1.0f);
|
||||
dtermCutFactor = dtermCutPercentInv + (dtermCutFactor * dtermCutPercent);
|
||||
}
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_D_CUT, 2, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
|
||||
} else if (axis == FD_PITCH) {
|
||||
DEBUG_SET(DEBUG_D_CUT, 3, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
|
||||
float dMinFactor = 1.0f;
|
||||
#if defined(USE_D_MIN)
|
||||
if (dMinPercent[axis] > 0) {
|
||||
float dMinGyroFactor = biquadFilterApply(&dMinRange[axis], delta);
|
||||
dMinGyroFactor = fabsf(dMinGyroFactor) * dMinGyroGain;
|
||||
const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * dMinSetpointGain;
|
||||
dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
|
||||
dMinFactor = dMinPercent[axis] + (1.0f - dMinPercent[axis]) * dMinFactor;
|
||||
dMinFactor = pt1FilterApply(&dMinLowpass[axis], dMinFactor);
|
||||
dMinFactor = MIN(dMinFactor, 1.0f);
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100));
|
||||
DEBUG_SET(DEBUG_D_MIN, 1, lrintf(dMinSetpointFactor * 100));
|
||||
DEBUG_SET(DEBUG_D_MIN, 2, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
|
||||
} else if (axis == FD_PITCH) {
|
||||
DEBUG_SET(DEBUG_D_MIN, 3, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dtermCutFactor;
|
||||
|
||||
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dMinFactor;
|
||||
} else {
|
||||
pidData[axis].D = 0;
|
||||
}
|
||||
previousGyroRateDterm[axis] = gyroRateDterm[axis];
|
||||
|
||||
// -----calculate feedforward component
|
||||
|
||||
// Only enable feedforward for rate mode and if launch control is inactive
|
||||
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
|
||||
|
||||
if (feedforwardGain > 0) {
|
||||
|
||||
// no transition if feedForwardTransition == 0
|
||||
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
|
||||
|
||||
float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
|
||||
|
||||
#if defined(USE_SMART_FEEDFORWARD)
|
||||
|
|
@ -1377,7 +1383,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
} else {
|
||||
pidData[axis].F = 0;
|
||||
}
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
if (yawSpinActive) {
|
||||
|
|
|
|||
|
|
@ -160,10 +160,11 @@ typedef struct pidProfile_s {
|
|||
uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated
|
||||
uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
|
||||
uint8_t thrustLinearization; // Compensation factor for pid linearization
|
||||
uint8_t dterm_cut_percent; // Amount to cut D by with no gyro activity, zero disables, 20 means cut 20%, 50 means cut 50%
|
||||
uint8_t dterm_cut_gain; // Gain factor for amount of gyro activity required to remove the dterm cut
|
||||
uint8_t dterm_cut_range_hz; // Biquad to prevent high frequency gyro noise from removing the dterm cut
|
||||
uint8_t dterm_cut_lowpass_hz; // First order lowpass to delay and smooth dterm cut factor
|
||||
uint8_t d_min_roll; // Minimum D value on roll axis
|
||||
uint8_t d_min_pitch; // Minimum D value on pitch axis
|
||||
uint8_t d_min_yaw; // Minimum D value on yaw axis
|
||||
uint8_t d_min_gain; // Gain factor for amount of gyro / setpoint activity required to boost D
|
||||
uint8_t d_min_advance; // Percentage multiplier for setpoint input to boost algorithm
|
||||
uint8_t motor_output_limit; // Upper limit of the motor output (percent)
|
||||
int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used
|
||||
} pidProfile_t;
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@
|
|||
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 8))
|
||||
#define USE_LAUNCH_CONTROL
|
||||
#define USE_DYN_LPF
|
||||
#define USE_D_CUT
|
||||
#define USE_D_MIN
|
||||
#endif
|
||||
|
||||
#if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 7))
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue