mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 05:15:25 +03:00
PID controller feedforward
Restructures the PID controller to decouple feedforward from D. Cleaned up the structure of the PID controller; moved some feature-based enhancements out of the main structure. Feedforward becomes a separate component of the PID controller and there is now: f_pitch f_roll f_yaw The default values of 60 for pitch and roll matches the default setpoint weight used in BF3.4. Yaw previously had no setpoint weight capability so the default here needs to be discussed. Currently it's also set to 60 and flight testing seems positive. Feedforward on yaw adds a lot of value so I don't think we want to default to 0. Instead we need decide on the default. All occurences of setpoint weight have been replaced by feedforward. "setpoint_relax_ratio" has been renamed to "feedforward_transition". The pidSum now consists of P + I + D + F. D has been added back for yaw (disabled by default with d_yaw = 0). We've found little need for D for normal quads but it may have value for other configurations - particularly tricopters. Updated CMS menus to support adjusting the feedforward for each axis. Changed the default for "rc_interp_ch" to be "RPYT". Need yaw to be smoothed to support feedforward. Open issues: Needs BFC support - Need to add support for the axis "F" gains. - Remove "setpoint weight" slider. - Rename "D Setpoint transition" to "Feedforward transition" Needs BBE support - Header "setpoint_relaxation_ratio" has been renamed "feedforward_transition" - Header "dterm_setpoint_weight" has been replaced with an array named "feed_forward_weight". example: H feed_forward_weight:65,60,60 (R,P,Y) - PID component "AXISF" has been added for all axes. Should be handled like P, I and D values. - PidSum calculation needs to include F. Needs LUA script support - Support the renamed "setpoint_relax_ratio". - Support for feedforward weight on all 3 axes. Open code issues: - rc_adjustments.c - support for adjusting feedforward weight for all axes. Currently only supporting roll - needs coordination with BFC.
This commit is contained in:
Bruce Luckcuck
parent
625b23915e
commit
17e76e48f6
13 changed files with 201 additions and 145 deletions
|
|
@ -104,22 +104,22 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
|||
|
||||
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
|
||||
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
|
||||
|
||||
void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
RESET_CONFIG(pidProfile_t, pidProfile,
|
||||
.pid = {
|
||||
[PID_ROLL] = { 46, 45, 25 },
|
||||
[PID_PITCH] = { 50, 50, 27 },
|
||||
[PID_YAW] = { 65, 45, 0 },
|
||||
[PID_ALT] = { 50, 0, 0 },
|
||||
[PID_POS] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
|
||||
[PID_POSR] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
|
||||
[PID_NAVR] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
|
||||
[PID_LEVEL] = { 50, 50, 75 },
|
||||
[PID_MAG] = { 40, 0, 0 },
|
||||
[PID_VEL] = { 55, 55, 75 }
|
||||
[PID_ROLL] = { 46, 45, 25, 60 },
|
||||
[PID_PITCH] = { 50, 50, 27, 60 },
|
||||
[PID_YAW] = { 65, 45, 0 , 60 },
|
||||
[PID_ALT] = { 50, 0, 0, 0 },
|
||||
[PID_POS] = { 15, 0, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
|
||||
[PID_POSR] = { 34, 14, 53, 0 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
|
||||
[PID_NAVR] = { 25, 33, 83, 0 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
|
||||
[PID_LEVEL] = { 50, 50, 75, 0 },
|
||||
[PID_MAG] = { 40, 0, 0, 0 },
|
||||
[PID_VEL] = { 55, 55, 75, 0 }
|
||||
},
|
||||
|
||||
.pidSumLimit = PIDSUM_LIMIT,
|
||||
|
|
@ -134,8 +134,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.vbatPidCompensation = 0,
|
||||
.pidAtMinThrottle = PID_STABILISATION_ON,
|
||||
.levelAngleLimit = 55,
|
||||
.setpointRelaxRatio = 0,
|
||||
.dtermSetpointWeight = 60,
|
||||
.feedForwardTransition = 0,
|
||||
.yawRateAccelLimit = 100,
|
||||
.rateAccelLimit = 0,
|
||||
.itermThrottleThreshold = 250,
|
||||
|
|
@ -209,11 +208,11 @@ typedef union dtermLowpass_u {
|
|||
} dtermLowpass_t;
|
||||
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[2];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
|
||||
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[2];
|
||||
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
|
|
@ -224,8 +223,8 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
|
|||
#endif
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
|
||||
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
|
||||
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
|
||||
|
|
@ -235,7 +234,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
|
|||
|
||||
void pidInitFilters(const pidProfile_t *pidProfile)
|
||||
{
|
||||
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
|
||||
BUILD_BUG_ON(FD_YAW != 2); // ensure yaw axis is 2
|
||||
|
||||
if (targetPidLooptime == 0) {
|
||||
// no looptime set, so set all the filters to null
|
||||
|
|
@ -261,7 +260,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
|
||||
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
} else {
|
||||
|
|
@ -273,7 +272,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
dtermLowpass2ApplyFn = nullFilterApply;
|
||||
} else {
|
||||
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
|
||||
}
|
||||
}
|
||||
|
|
@ -287,13 +286,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
break;
|
||||
case FILTER_PT1:
|
||||
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
|
||||
}
|
||||
break;
|
||||
case FILTER_BIQUAD:
|
||||
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
|
|
@ -328,7 +327,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
rcSmoothingFilterType = filterType;
|
||||
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
setpointDerivativeLpfInitialized = true;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
||||
|
|
@ -344,7 +343,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
|
||||
{
|
||||
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
||||
|
|
@ -362,12 +361,12 @@ typedef struct pidCoefficient_s {
|
|||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
float Kf;
|
||||
} pidCoefficient_t;
|
||||
|
||||
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3];
|
||||
static FAST_RAM_ZERO_INIT float maxVelocity[3];
|
||||
static FAST_RAM_ZERO_INIT float relaxFactor;
|
||||
static FAST_RAM_ZERO_INIT float dtermSetpointWeight;
|
||||
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT float feedForwardTransition;
|
||||
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
||||
static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
|
||||
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
|
||||
|
|
@ -424,18 +423,18 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
|
|||
|
||||
void pidInitConfig(const pidProfile_t *pidProfile)
|
||||
{
|
||||
if (pidProfile->feedForwardTransition == 0) {
|
||||
feedForwardTransition = 0;
|
||||
} else {
|
||||
feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
|
||||
}
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
|
||||
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
|
||||
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
|
||||
pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
|
||||
}
|
||||
|
||||
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f;
|
||||
if (pidProfile->setpointRelaxRatio == 0) {
|
||||
relaxFactor = 0;
|
||||
} else {
|
||||
relaxFactor = 100.0f / pidProfile->setpointRelaxRatio;
|
||||
}
|
||||
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
||||
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
|
||||
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
|
||||
|
|
@ -598,7 +597,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
|
|||
|
||||
static float accelerationLimit(int axis, float currentPidSetpoint)
|
||||
{
|
||||
static float previousSetpoint[3];
|
||||
static float previousSetpoint[XYZ_AXIS_COUNT];
|
||||
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
|
||||
|
||||
if (ABS(currentVelocity) > maxVelocity[axis]) {
|
||||
|
|
@ -701,7 +700,7 @@ static void rotateITermAndAxisError()
|
|||
#endif
|
||||
) {
|
||||
const float gyroToAngle = dT * RAD;
|
||||
float rotationRads[3];
|
||||
float rotationRads[XYZ_AXIS_COUNT];
|
||||
for (int i = FD_ROLL; i <= FD_YAW; i++) {
|
||||
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
|
||||
}
|
||||
|
|
@ -711,7 +710,7 @@ static void rotateITermAndAxisError()
|
|||
}
|
||||
#endif
|
||||
if (itermRotation) {
|
||||
float v[3];
|
||||
float v[XYZ_AXIS_COUNT];
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
v[i] = pidData[i].I;
|
||||
}
|
||||
|
|
@ -797,12 +796,51 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
|
|||
}
|
||||
#endif // USE_ACRO_TRAINER
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
|
||||
{
|
||||
float ret = pidSetpointDelta;
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
|
||||
}
|
||||
if (setpointDerivativeLpfInitialized) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
|
||||
break;
|
||||
}
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
#ifdef USE_SMART_FEEDFORWARD
|
||||
void FAST_CODE applySmartFeedforward(int axis)
|
||||
{
|
||||
if (smartFeedforward) {
|
||||
if (pidData[axis].P * pidData[axis].F > 0) {
|
||||
if (ABS(pidData[axis].F) > ABS(pidData[axis].P)) {
|
||||
pidData[axis].P = 0;
|
||||
} else {
|
||||
pidData[axis].F = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // USE_SMART_FEEDFORWARD
|
||||
|
||||
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
|
||||
// Based on 2DOF reference design (matlab)
|
||||
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
|
||||
{
|
||||
static float previousGyroRateDterm[2];
|
||||
static float previousPidSetpoint[2];
|
||||
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
|
||||
static float previousPidSetpoint[XYZ_AXIS_COUNT];
|
||||
|
||||
const float tpaFactor = getThrottlePIDAttenuation();
|
||||
const float motorMixRange = getMotorMixRange();
|
||||
|
|
@ -820,12 +858,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
// gradually scale back integration when above windup point
|
||||
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
|
||||
|
||||
// Dynamic d component, enable 2-DOF PID controller only for rate mode
|
||||
const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
|
||||
|
||||
// Precalculate gyro deta for D-term here, this allows loop unrolling
|
||||
float gyroRateDterm[2];
|
||||
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) {
|
||||
float gyroRateDterm[XYZ_AXIS_COUNT];
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
|
||||
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
|
||||
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
|
||||
|
|
@ -835,12 +870,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
|
||||
// ----------PID controller----------
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
|
||||
float currentPidSetpoint = getSetpointRate(axis);
|
||||
if (maxVelocity[axis]) {
|
||||
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
|
||||
}
|
||||
// Yaw control is GYRO based, direct sticks control is applied to rate PID
|
||||
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != YAW) {
|
||||
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != FD_YAW) {
|
||||
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
|
||||
}
|
||||
|
||||
|
|
@ -937,7 +973,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
|
||||
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
||||
// 2-DOF PID controller with optional filter on derivative term.
|
||||
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
|
||||
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
|
||||
|
||||
// -----calculate P component and add Dynamic Part based on stick input
|
||||
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
|
||||
|
|
@ -954,9 +990,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
}
|
||||
|
||||
// -----calculate D component
|
||||
if (axis != FD_YAW) {
|
||||
// no transition if relaxFactor == 0
|
||||
float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1;
|
||||
if (pidCoefficient[axis].Kd > 0) {
|
||||
|
||||
// Divide rate change by dT to get differential (ie dr/dt).
|
||||
// dT is fixed and calculated from the target PID loop time
|
||||
|
|
@ -969,73 +1003,52 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
|
||||
|
||||
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
|
||||
} else {
|
||||
pidData[axis].D = 0;
|
||||
}
|
||||
previousGyroRateDterm[axis] = gyroRateDterm[axis];
|
||||
|
||||
// -----calculate feedforward component
|
||||
|
||||
// Only enable feedforward for rate mode
|
||||
const float feedforwardGain = flightModeFlags ? 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
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
|
||||
}
|
||||
if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
|
||||
break;
|
||||
}
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
|
||||
}
|
||||
}
|
||||
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
const float pidFeedForward =
|
||||
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
|
||||
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
|
||||
|
||||
#if defined(USE_SMART_FEEDFORWARD)
|
||||
bool addFeedforward = true;
|
||||
if (smartFeedforward) {
|
||||
if (pidData[axis].P * pidFeedForward > 0) {
|
||||
if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
|
||||
pidData[axis].P = 0;
|
||||
} else {
|
||||
addFeedforward = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (addFeedforward)
|
||||
applySmartFeedforward(axis);
|
||||
#endif
|
||||
{
|
||||
pidData[axis].D += pidFeedForward;
|
||||
}
|
||||
previousGyroRateDterm[axis] = gyroRateDterm[axis];
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
} else {
|
||||
pidData[axis].F = 0;
|
||||
}
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
if (yawSpinActive) {
|
||||
if (yawSpinActive) {
|
||||
pidData[axis].I = 0; // in yaw spin always disable I
|
||||
if (axis <= FD_PITCH) {
|
||||
// zero PIDs on pitch and roll leaving yaw P to correct spin
|
||||
pidData[axis].P = 0;
|
||||
pidData[axis].I = 0;
|
||||
pidData[axis].D = 0;
|
||||
pidData[axis].F = 0;
|
||||
}
|
||||
#endif // USE_YAW_SPIN_RECOVERY
|
||||
}
|
||||
}
|
||||
|
||||
// calculating the PID sum
|
||||
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
|
||||
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
|
||||
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
if (yawSpinActive) {
|
||||
// yaw P alone to correct spin
|
||||
pidData[FD_YAW].I = 0;
|
||||
}
|
||||
#endif // USE_YAW_SPIN_RECOVERY
|
||||
|
||||
// YAW has no D
|
||||
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;
|
||||
// calculating the PID sum
|
||||
pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
|
||||
}
|
||||
|
||||
// Disable PID control if at zero throttle or if gyro overflow detected
|
||||
// This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
|
||||
|
|
@ -1044,6 +1057,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
pidData[axis].P = 0;
|
||||
pidData[axis].I = 0;
|
||||
pidData[axis].D = 0;
|
||||
pidData[axis].F = 0;
|
||||
|
||||
pidData[axis].Sum = 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue