BetaFlight模块设计之三十三：Pid模块分析
基于BetaFlight开源代码框架简介的框架设计，逐步分析内部模块功能设计。
Pid模块
描述：目前主要用于文件系统的相关维护例程。
从代码资源角度Pid模块可以分为三个子模块：
- Pid_init子模块
- Pid_audio子模块
- Pid算法子模块
Pid模块代码
├──> src\main\flight
│ ├──> Pid.c
│ └──> Pid.h
│ ├──> Pid_init.c
│ └──> Pid_init.h
└──> src\main\io
├──> Pidaudio.c
└──> Pidaudio.h
1. Pid_audio子模块
根据PID的工作状态，FC在耳机上增加一个声音，让飞控操作人员更加了解PID运行状态。
- PIDSUM_X: Roll
- PIDSUM_Y: Pitch
- PIDSUM_XY: (Roll + Pitch) / 2
typedef enum {
PID_AUDIO_OFF = 0,
PID_AUDIO_PIDSUM_X,
PID_AUDIO_PIDSUM_Y,
PID_AUDIO_PIDSUM_XY,
} pidAudioModes_e;
void pidAudioUpdate(void);
void pidAudioInit(void);
pidAudioModes_e pidAudioGetMode(void);
void pidAudioSetMode(pidAudioModes_e mode);
以下机型使用该子模块：
./src/main/target/SPRACINGF7DUAL/target.h:183:#define USE_PID_AUDIO
./src/main/target/SPRACINGH7EXTREME/target.h:171:#define USE_PID_AUDIO
注1：SPRACINGF7DUAL机型，BetaFlight支持情况介绍
注2：SPRACINGH7EXTREME机型
2. Pid_init子模块
Pid算法初始化pidInit函数(包含了pidInitFilters和pidInitConfig)。
void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
针对USE_RC_SMOOTHING_FILTER下pt3Filter_t feedforwardPt3[XYZ_AXIS_COUNT]进行pt3FilterGain(截止频率和dT)更新。
void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateFeedforwardLpf(uint16_t filterCutoff);
处理CMS菜单复制Pid Profile的执行函数。
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
3. Pid算法子模块
BetaFlight模块设计之二十八：MainPidLoop任务分析中最核心的是Pid算法。
通过Pid_init子模块对算法进行了全局(变量)初始化，在MainPidLoop任务中，通过subTaskPidController子任务调用pidController算法函数。
// 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 pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
注：这里也提到了一点“specialised for current (mini) multirotor usage”，对大机架的支持也许并没有那么好，比如：F450
3.1 TPA模式
TPA 是Throttle PID Attenuation的缩写。
tpaFactor主要将会影响PD参量，减少油门越大带入的干扰越大的情况。
typedef enum {
TPA_MODE_PD, //影响PD参数
TPA_MODE_D //仅影响D参数(默认配置)
} tpaMode_e;
pidUpdateTpaFactor
├──> tpaBreakpoint = (currentControlRateProfile->tpa_breakpoint - 1000) / 1000.0f;
├──> tpaRate = currentControlRateProfile->tpa_rate / 100.0f;
├──> <throttle > tpaBreakpoint><throttle < 1.0f>
│ └──> tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint); //超过tpaBreakpoint时，随着油门增加，tpaFactor逐渐线性降低至1-tpaRate
├──> <!(throttle > tpaBreakpoint)> //没有超过tpaBreakpoint时，tpaFactor=1
│ └──> tpaRate = 0.0f;
└──> pidRuntime.tpaFactor = 1.0f - tpaRate;
3.2 飞行模式
从代码角度分析，实际飞控存在以下三种飞行模式：
- LEVEL_MODE_OFF：手动(Acro)
- LEVEL_MODE_R：Pitch手动，Roll自稳(NFE level race mode)
- LEVEL_MODE_RP：Pitch + Roll自稳(Angle, Horizon)
typedef enum {
LEVEL_MODE_OFF = 0,
LEVEL_MODE_R,
LEVEL_MODE_RP,
} levelMode_e;
pidLevel // LEVEL_MODE_RP: Angle, Horizon
├──> angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis);
├──> <USE_GPS_RESCUE>
│ └──> angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
├──> angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
├──> errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
├──> <FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)>
│ └──> currentPidSetpoint = errorAngle * pidRuntime.levelGain; // ANGLE mode - control is angle based
├──> <!(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE))>
│ ├──> horizonLevelStrength = calcHorizonLevelStrength();
│ └──> currentPidSetpoint = currentPidSetpoint + (errorAngle * pidRuntime.horizonGain * horizonLevelStrength);
└──> return currentPidSetpoint;
3.3 Launch模式
主要用于方向性的可控起飞。
typedef enum {
LAUNCH_CONTROL_MODE_NORMAL = 0,
LAUNCH_CONTROL_MODE_PITCHONLY,
LAUNCH_CONTROL_MODE_FULL,
LAUNCH_CONTROL_MODE_COUNT // must be the last element
} launchControlMode_e;
#define LAUNCH_CONTROL_MAX_RATE 100.0f
#define LAUNCH_CONTROL_MIN_RATE 5.0f
#define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
// processing power that it should be a non-issue.
static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
{
float ret = 0.0f;
// Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
// reached at 50% stick deflection. This keeps the launch control positioning consistent
// regardless of the user's rates.
if ((axis == FD_PITCH) || (pidRuntime.launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
}
#if defined(USE_ACC)
// If ACC is enabled and a limit angle is set, then try to limit forward tilt
// to that angle and slow down the rate as the limit is approached to reduce overshoot
if ((axis == FD_PITCH) && (pidRuntime.launchControlAngleLimit > 0) && (ret > 0)) {
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
if (currentAngle >= pidRuntime.launchControlAngleLimit) {
ret = 0.0f;
} else {
//for the last 10 degrees scale the rate from the current input to 5 dps
const float angleDelta = pidRuntime.launchControlAngleLimit - currentAngle;
if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
}
}
}
#else
UNUSED(angleTrim);
#endif
return ret;
}
Launch control相关设置目前只能通过Cli命令行，详见链接。
set launch_control_mode = PITCHONLY
set launch_trigger_allow_reset = ON
set launch_trigger_throttle_percent = 30
set launch_angle_limit = 8
set osd_warn_launch_control = ON
3.4 AcroTrainer模式
该模式主要为了用于训练Acro手动模式，避免炸机的一种模式。
pidAcroTrainerInit
├──> pidRuntime.acroTrainerAxisState[FD_ROLL] = 0;
└──> pidRuntime.acroTrainerAxisState[FD_PITCH] = 0;
pidSetAcroTrainerState
└──> <pidRuntime.acroTrainerActive != newState>
├──> <newState>
│ └──> pidAcroTrainerInit
└──> pidRuntime.acroTrainerActive = newState;
// Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
// There are three states:
// 1. Current angle has exceeded limit
// Apply correction to return to limit (similar to pidLevel)
// 2. Future overflow has been projected based on current angle and gyro rate
// Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
// 3. If no potential overflow is detected, then return the original setPoint
applyAcroTrainer
├──> <!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)>
│ ├──> setpointSign = acroTrainerSign(setPoint);
│ ├──> angleSign = acroTrainerSign(currentAngle);
│ ├──> currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
│ ├──> <(pidRuntime.acroTrainerAxisState[axis] != 0) && (pidRuntime.acroTrainerAxisState[axis] != setpointSign>
│ │ └──> pidRuntime.acroTrainerAxisState[axis] = 0;
----- 角度超过允许最大值：重置I
│ ├──> <(fabsf(currentAngle) > pidRuntime.acroTrainerAngleLimit) && (pidRuntime.acroTrainerAxisState[axis] == 0)><angleSign == setpointSign>
│ │ ├──> pidRuntime.acroTrainerAxisState[axis] = angleSign;
│ │ └──> resetIterm = true;
----- 角度超过允许最大值：纠偏返回setpoint
│ ├──> <pidRuntime.acroTrainerAxisState[axis] != 0>
│ │ └──> ret = constrainf(((pidRuntime.acroTrainerAngleLimit * angleSign) - currentAngle) * pidRuntime.acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
----- 角度预测：若超出重置I，返回更新后的setpoint；反之返回原始setpoint。
│ ├──> <pidRuntime.acroTrainerAxisState[axis] == 0>
│ │ ├──> checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * pidRuntime.acroTrainerLookaheadTime;
│ │ ├──> projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
│ │ ├──> projectedAngleSign = acroTrainerSign(projectedAngle); //若条件不满足，则满足训练条件
│ │ └──> <(fabsf(projectedAngle) > pidRuntime.acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)>
│ │ ├──> ret = ((pidRuntime.acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * pidRuntime.acroTrainerGain;
│ │ └──> resetIterm = true;
│ └──> <resetIterm>
│ └──> pidData[axis].I = 0;
└──> return setPoint
3.5 antiGravity模式
主要是为了解决突然加速和减速对飞机稳定性的影响：
- 在processRcCommand处理中，checkForThrottleErrorResetState会根据Rc命令变化量，在ANTI_GRAVITY_STEP模式下调整pidRuntime.itermAccelerator；
- 在subTaskMotorUpdate的mixTable中，pidUpdateAntiGravityThrottleFilter在ANTI_GRAVITY_SMOOTH调整更新pidRuntime.antiGravityPBoost；
- pidController将agGain叠加影响到I-Term上，影响稳定性；
typedef enum {
ANTI_GRAVITY_SMOOTH, //默认状态
ANTI_GRAVITY_STEP
} antiGravityMode_e;
checkForThrottleErrorResetState
├──> throttleVelocityThreshold = (featureIsEnabled(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
├──> rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
├──> rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
└──> <currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP>
├──> <ABS(rcCommandSpeed) > throttleVelocityThreshold>
│ └──> pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
└──> <!(ABS(rcCommandSpeed) > throttleVelocityThreshold)>
└──> pidSetItermAccelerator(0.0f);
pidUpdateAntiGravityThrottleFilter
└──> <pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH>
├──> antiGravityThrottleLpf = pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle);
├──> <throttle < 0.5f>
│ └──> pidRuntime.antiGravityPBoost = 0.5f - throttle; // focus P boost on low throttle range only
├──> <!(throttle < 0.5f)>
│ └──> pidRuntime.antiGravityPBoost = 0.0f;
├──> <antiGravityThrottleLpf < throttle>
│ └──> pidRuntime.antiGravityPBoost *= 0.5f; // use lowpass to identify start of a throttle up, use this to reduce boost at start by half
├──> pidRuntime.antiGravityThrottleHpf = fabsf(throttle - antiGravityThrottleLpf);
├──> pidRuntime.antiGravityPBoost = pidRuntime.antiGravityPBoost * pidRuntime.antiGravityThrottleHpf; // high-passed throttle focuses boost on faster throttle changes
└──> pidRuntime.antiGravityPBoost = pt1FilterApply(&pidRuntime.antiGravitySmoothLpf, pidRuntime.antiGravityPBoost); // smooth the P boost at 3hz to remove the jagged edges and prolong the effect after throttle stops
3.6 pidController函数
PID运算主要在pidController这个函数中实现，但由于历史及全局变量诸多问题，一上来就从整体来看是比较复杂的。
这里做了一些精简和排序，并将部分特性相关的内容抽取出来介绍，希望后续进一步结合实际效果来看每个特性对PID的细节影响。
PID控制算法外，还增加了以下特性：
- FeedForward特性
- antiGravity特性
- TPA特性
- AutoLevel特性(Race Mode, Angle Mode, Horizon Mode)
- Launch特性
pidController
├──> [ -----Basic status and settings ]
│ ├──> angleTrim = &accelerometerConfig()->accelerometerTrims;
│ ├──> <USE_YAW_SPIN_RECOVERY>
│ │ └──> yawSpinActive = gyroYawSpinDetected();
│ ├──> launchControlActive = isLaunchControlActive();
│ └──> <USE_ACC>
│ ├──> gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
│ ├──> <FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive>
│ │ ├──> <pidRuntime.levelRaceMode && !gpsRescueIsActive>
│ │ │ └──> levelMode = LEVEL_MODE_R;
│ │ └──> <!(pidRuntime.levelRaceMode && !gpsRescueIsActive)>
│ │ └──> levelMode = LEVEL_MODE_RP;
│ ├──> <!(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive)>
│ │ └──>levelMode = LEVEL_MODE_OFF;
│ ├──> <levelMode><(levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)>
│ │ └──> levelModeStartTimeUs = currentTimeUs;
│ ├──> <!levelMode>
│ │ └──> levelModeStartTimeUs = 0;
│ └──> gpsRescuePreviousState = gpsRescueIsActive;
│
├──> [ -----Throttle PID Attenuation for P ] // pidUpdateTpaFactor update with throttle position
│ ├──> <USE_TPA_MODE>
│ │ └──> tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
│ └──> <!USE_TPA_MODE>
│ └──> tpaFactorKp = pidRuntime.tpaFactor;
│
├──> [ -----calculate dynamic i component ] // boost I for antigravity
│ ├──> <(pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled>
│ │ ├──> pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain;
│ │ ├──> pidRuntime.antiGravityPBoost *= pidRuntime.itermAcceleratorGain; // users AG Gain changes P boost
│ │ ├──> pidRuntime.itermAccelerator += pidRuntime.antiGravityPBoost * 0.05f; // add some percentage of that slower, longer acting P boost factor to prolong AG effect on iTerm
│ │ └──> pidRuntime.antiGravityPBoost *= 0.02f;
│ ├──> <!((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled)>
│ │ └──> pidRuntime.antiGravityPBoost = 0.0f;
│ └──> agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
│
├──> dynCi = pidRuntime.dT;
├──> <pidRuntime.itermWindupPointInv > 1.0f>
│ └──> dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
├──> <for (int axis = FD_ROLL; axis <= FD_YAW; ++axis)> //Precalculate gyro deta for D-term
│ ├──> gyroRateDterm[axis] = gyro.gyroADCf[axis];
│ ├──> gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
│ ├──> gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *) &pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]);
│ └──> gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]);
├──> rotateItermAndAxisError();
├──> <USE_RPM_FILTER>
│ └──> rpmFilterUpdate
├──> <USE_FEEDFORWARD>
│ ├──> newRcFrame = false;
│ └──> <getShouldUpdateFeedforward()>
│ └──> newRcFrame = true;
│
├──> <for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) // ----------PID controller----------
│ ├──> [ -----calculate errorRate ]
│ │ ├──> currentPidSetpoint = getSetpointRate(axis);
│ │ ├──> <pidRuntime.maxVelocity[axis]>
│ │ │ └──> currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
│ │ ├──> <USE_ACC>
│ │ │ ├──> <LEVEL_MODE_OFF>
│ │ │ │ └──> break;
│ │ │ ├──> <LEVEL_MODE_R><axis == FD_PITCH>
│ │ │ │ └──> break;
│ │ │ ├──> <LEVEL_MODE_RP><axis == FD_YAW>
│ │ │ │ └──> break;
│ │ │ └──> currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); // LEVEL_MODE_RP: Angle, Horizon
│ │ ├──> <USE_ACRO_TRAINER><(axis != FD_YAW) && pidRuntime.acroTrainerActive && !pidRuntime.inCrashRecoveryMode && !launchControlActive>
│ │ │ └──> currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
│ │ ├──> <USE_LAUNCH_CONTROL><launchControlActive>
│ │ │ ├──> <USE_ACC>
│ │ │ │ └──> currentPidSetpoint = applyLaunchControl(axis, angleTrim);
│ │ │ └──> <!USE_ACC>
│ │ │ └──> currentPidSetpoint = applyLaunchControl(axis, NULL);
│ │ ├──> <USE_YAW_SPIN_RECOVERY><(axis == FD_YAW) && yawSpinActive>
│ │ │ └──> currentPidSetpoint = 0.0f;
│ │ ├──> gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
│ │ ├──> errorRate = currentPidSetpoint - gyroRate; // r - y
│ │ ├──> <USE_ACC>
│ │ │ └──> handleCrashRecovery(pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, ¤tPidSetpoint, &errorRate);
│ │ ├──> previousIterm = pidData[axis].I;
│ │ ├──> itermErrorRate = errorRate;
│ │ ├──> <USE_ABSOLUTE_CONTROL>
│ │ │ └──> uncorrectedSetpoint = currentPidSetpoint;
│ │ ├──> <USE_ITERM_RELAX><!launchControlActive && !pidRuntime.inCrashRecoveryMode>
│ │ │ ├──> applyItermRelax(axis, previousIterm, gyroRate, &itermErrorRate, ¤tPidSetpoint);
│ │ │ └──> errorRate = currentPidSetpoint - gyroRate;
│ │ └──> <USE_ABSOLUTE_CONTROL>
│ │ └──> setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
│ │
│ ├──> <USE_FEEDFORWARD>
│ │ └──> pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging);
│ ├──> pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
│ │
│ ├──> [ -----calculate P component ]
│ │ ├──> pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
│ │ └──> <axis == FD_YAW>
│ │ └──> pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
│ │
│ ├──> [ -----calculate I component ]
│ │ ├──> <launchControlActive>
│ │ │ ├──> Ki = pidRuntime.launchControlKi;
│ │ │ └──> axisDynCi = dynCi;
│ │ ├──> <!launchControlActive>
│ │ │ ├──> Ki = pidRuntime.pidCoefficient[axis].Ki;
│ │ │ └──> axisDynCi = (axis == FD_YAW) ? dynCi : pidRuntime.dT; // only apply windup protection to yaw
│ │ └──> pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + agGain) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit);
│ │
│ ├──> [ -----calculate D component ]
│ │ ├──> <(pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive>
│ │ │ ├──> delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
│ │ │ ├──> preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta;
│ │ │ ├──> <USE_ACC><cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US>
│ │ │ │ └──> detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
│ │ │ ├──> <USE_D_MIN>
│ │ │ │ ├──> dMinFactor = 1.0f;
│ │ │ │ ├──> <pidRuntime.dMinPercent[axis] > 0>
│ │ │ │ └──> preTpaD *= dMinFactor;
│ │ │ └──> pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
│ │ ├──> <!((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive)>
│ │ │ └──> pidData[axis].D = 0;
│ │ └──> pidData[axis].D = 0;previousGyroRateDterm[axis] = gyroRateDterm[axis];
│ ├──> <USE_ABSOLUTE_CONTROL>
│ │ ├──> pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
│ │ └──> pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
│ │
│ ├──> [ -----calculate feedforward component ]
│ │ ├──> feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
│ │ ├──> <feedforwardGain > 0>
│ │ │ ├──> feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
│ │ │ ├──> feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
│ │ │ ├──> <USE_FEEDFORWARD>
│ │ │ │ └──> pidData[axis].F = shouldApplyFeedforwardLimits(axis) ? applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
│ │ │ ├──> <!USE_FEEDFORWARD>
│ │ │ │ └──> pidData[axis].F = feedForward;
│ │ │ └──> <USE_RC_SMOOTHING_FILTER>
│ │ │ └──> pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
│ │ └──> <!(feedforwardGain > 0)>
│ │ └──> pidData[axis].F = 0;
│ ├──> <USE_YAW_SPIN_RECOVERY><yawSpinActive>
│ │ ├──> pidData[axis].I = 0; // in yaw spin always disable I
│ │ └──> <axis <= FD_PITCH>
│ │ └──> pidData[axis].P = 0;pidData[axis].D = 0;pidData[axis].F = 0;
│ ├──> <USE_LAUNCH_CONTROL><launchControlActive>
│ │ ├──> launchControlYawItermLimit = (pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
│ │ └──> pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit);
│ │ └──> <pidRuntime.launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY>
│ │ └──> pidData[FD_ROLL].P = 0;pidData[FD_ROLL].I = 0;pidData[FD_YAW].P = 0;pidData[FD_PITCH].I = MAX(0.0f, pidData[FD_PITCH].I);
│ │
│ └──> [ -----calculate pid sum ]
│ ├──> agBoostAttenuator = fabsf(currentPidSetpoint) / 50.0f;
│ ├──> agBoostAttenuator = MAX(agBoostAttenuator, 1.0f);
│ ├──> agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator);
│ ├──> <axis != FD_YAW>
│ │ └──> pidData[axis].P *= agBoost;
│ ├──> pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
│ ├──> <USE_INTEGRATED_YAW_CONTROL>
│ │ ├──> <axis == FD_YAW && pidRuntime.useIntegratedYaw>
│ │ │ ├──> pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
│ │ │ └──> pidData[axis].Sum -= pidData[axis].Sum * pidRuntime.integratedYawRelax / 100000.0f * pidRuntime.dT / 0.000125f;
│ │ └──> <!(axis == FD_YAW && pidRuntime.useIntegratedYaw)>
│ │ └──> pidData[axis].Sum = pidSum;
│ └──> <!USE_INTEGRATED_YAW_CONTROL>
│ └──> pidData[axis].Sum = pidSum;
│
├──> <!pidRuntime.pidStabilisationEnabled || gyroOverflowDetected()>
│ └──> <for (int axis = FD_ROLL; axis <= FD_YAW; ++axis)>
│ └──> pidData[axis].P = 0;pidData[axis].I = 0;pidData[axis].D = 0;pidData[axis].F = 0;pidData[axis].Sum = 0;
└──> <pidRuntime.zeroThrottleItermReset>
└──>pidResetIterm
336
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
