BetaFlight模块设计之三十三:Pid模块分析
- Pid模块
- 1. Pid_audio子模块
- 2. Pid_init子模块
- 3. Pid算法子模块
- 3.1 TPA模式
- 3.2 飞行模式
- 3.3 Launch模式
- 3.4 AcroTrainer模式
- 3.5 antiGravity模式
- 3.6 pidController函数
基于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算法函数。
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,
TPA_MODE_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);
├──> <!(throttle > tpaBreakpoint)>
│ └──> 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
├──> angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis);
├──> <USE_GPS_RESCUE>
│ └──> angle += gpsRescueAngle[axis] / 100;
├──> 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;
├──> <!(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
} launchControlMode_e;
#define LAUNCH_CONTROL_MAX_RATE 100.0f
#define LAUNCH_CONTROL_MIN_RATE 5.0f
#define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f
static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
{
float ret = 0.0f;
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 ((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 {
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;
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;
├──> <!(throttle < 0.5f)>
│ └──> pidRuntime.antiGravityPBoost = 0.0f;
├──> <antiGravityThrottleLpf < throttle>
│ └──> pidRuntime.antiGravityPBoost *= 0.5f;
├──> pidRuntime.antiGravityThrottleHpf = fabsf(throttle - antiGravityThrottleLpf);
├──> pidRuntime.antiGravityPBoost = pidRuntime.antiGravityPBoost * pidRuntime.antiGravityThrottleHpf;
└──> pidRuntime.antiGravityPBoost = pt1FilterApply(&pidRuntime.antiGravitySmoothLpf, pidRuntime.antiGravityPBoost);
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 ]
│ ├──> <USE_TPA_MODE>
│ │ └──> tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
│ └──> <!USE_TPA_MODE>
│ └──> tpaFactorKp = pidRuntime.tpaFactor;
│
├──> [ -----calculate dynamic i component ]
│ ├──> <(pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled>
│ │ ├──> pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain;
│ │ ├──> pidRuntime.antiGravityPBoost *= pidRuntime.itermAcceleratorGain;
│ │ ├──> pidRuntime.itermAccelerator += pidRuntime.antiGravityPBoost * 0.05f;
│ │ └──> 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)>
│ ├──> 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)
│ ├──> [ -----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);
│ │ ├──> <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];
│ │ ├──> errorRate = currentPidSetpoint - gyroRate;
│ │ ├──> <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;
│ │ └──> 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;
│ │ └──> <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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)