Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Tidy of pid code

Browse source
This commit is contained in:
Martin Budden 2017-07-27 09:57:02 +01:00
parent 6a8678aceb
commit c1f74c9aed
1 changed files with 29 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

53
src/main/flight/pid.c
View file

@ -139,7 +139,8 @@ void pidResetErrorGyroState(void)
static float itermAccelerator = 1.0f; static float itermAccelerator = 1.0f;
void pidSetItermAccelerator(float newItermAccelerator) { void pidSetItermAccelerator(float newItermAccelerator)
{
itermAccelerator = newItermAccelerator; itermAccelerator = newItermAccelerator;
} }
@ -167,7 +168,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static firFilterDenoise_t denoisingFilter[2]; static firFilterDenoise_t denoisingFilter[2];
static pt1Filter_t pt1FilterYaw; static pt1Filter_t pt1FilterYaw;
uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed
float dTermNotchHz; float dTermNotchHz;
if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) { if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
@ -180,15 +181,15 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
} }
if (!dTermNotchHz) { if (dTermNotchHz) {
dtermNotchFilterApplyFn = nullFilterApply;
} else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); 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_PITCH; axis++) {
dtermFilterNotch[axis] = &biquadFilterNotch[axis]; dtermFilterNotch[axis] = &biquadFilterNotch[axis];
biquadFilterInit(dtermFilterNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(dtermFilterNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
} }
} else {
dtermNotchFilterApplyFn = nullFilterApply;
} }
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) { if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
@ -231,11 +232,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
} }
static float Kp[3], Ki[3], Kd[3], maxVelocity[3]; static float Kp[3], Ki[3], Kd[3];
static float maxVelocity[3];
static float relaxFactor; static float relaxFactor;
static float dtermSetpointWeight; static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
horizonFactorRatio, ITermWindupPoint, ITermWindupPointInv; static float ITermWindupPoint, ITermWindupPointInv;
static uint8_t horizonTiltExpertMode; static uint8_t horizonTiltExpertMode;
static timeDelta_t crashTimeLimitUs; static timeDelta_t crashTimeLimitUs;
static int32_t crashRecoveryAngleDeciDegrees; static int32_t crashRecoveryAngleDeciDegrees;
@ -244,7 +246,8 @@ static float crashDtermThreshold;
static float crashGyroThreshold; static float crashGyroThreshold;
static float crashSetpointThreshold; static float crashSetpointThreshold;
void pidInitConfig(const pidProfile_t *pidProfile) { void pidInitConfig(const pidProfile_t *pidProfile)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P; Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P;
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
@ -279,14 +282,13 @@ void pidInit(const pidProfile_t *pidProfile)
} }
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
static float calcHorizonLevelStrength() { static float calcHorizonLevelStrength(void)
{
// start with 1.0 at center stick, 0.0 at max stick deflection: // start with 1.0 at center stick, 0.0 at max stick deflection:
float horizonLevelStrength = 1.0f - float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
// 0 at level, 90 at vertical, 180 at inverted (degrees): // 0 at level, 90 at vertical, 180 at inverted (degrees):
const float currentInclination = MAX(ABS(attitude.values.roll), const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f;
ABS(attitude.values.pitch)) / 10.0f;
// horizonTiltExpertMode: 0 = leveling always active when sticks centered, // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
// 1 = leveling can be totally off when inverted // 1 = leveling can be totally off when inverted
@ -296,34 +298,31 @@ static float calcHorizonLevelStrength() {
// horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero) // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling) // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
// for larger inclinations; 0.0 at horizonCutoffDegrees value: // for larger inclinations; 0.0 at horizonCutoffDegrees value:
const float inclinationLevelRatio = constrainf( const float inclinationLevelRatio = constrainf((horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1);
(horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1);
// apply configured horizon sensitivity: // apply configured horizon sensitivity:
// when stick is near center (horizonLevelStrength ~= 1.0) // when stick is near center (horizonLevelStrength ~= 1.0)
// H_sensitivity value has little effect, // H_sensitivity value has little effect,
// when stick is deflected (horizonLevelStrength near 0.0) // when stick is deflected (horizonLevelStrength near 0.0)
// H_sensitivity value has more effect: // H_sensitivity value has more effect:
horizonLevelStrength = (horizonLevelStrength - 1) * horizonLevelStrength = (horizonLevelStrength - 1) * 100 / horizonTransition + 1;
100 / horizonTransition + 1;
// apply inclination ratio, which may lower leveling // apply inclination ratio, which may lower leveling
// to zero regardless of stick position: // to zero regardless of stick position:
horizonLevelStrength *= inclinationLevelRatio; horizonLevelStrength *= inclinationLevelRatio;
} } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
else // d_level=0 or horizon_tilt_effect>=175 means no leveling
horizonLevelStrength = 0; horizonLevelStrength = 0;
}
} else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered) } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
float sensitFact; float sensitFact;
if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0 if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0
// horizonFactorRatio: 1.0 to 0.0 (larger means more leveling) // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling) // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
// for larger inclinations, goes to 1.0 at inclination==level: // for larger inclinations, goes to 1.0 at inclination==level:
const float inclinationLevelRatio = (180-currentInclination)/180 * const float inclinationLevelRatio = (180-currentInclination)/180 * (1.0f-horizonFactorRatio) + horizonFactorRatio;
(1.0f-horizonFactorRatio) + horizonFactorRatio;
// apply ratio to configured horizon sensitivity: // apply ratio to configured horizon sensitivity:
sensitFact = horizonTransition * inclinationLevelRatio; sensitFact = horizonTransition * inclinationLevelRatio;
} } else { // horizonTiltEffect=0 for "old" functionality
else // horizonTiltEffect=0 for "old" functionality
sensitFact = horizonTransition; sensitFact = horizonTransition;
}
if (sensitFact <= 0) { // zero means no leveling if (sensitFact <= 0) { // zero means no leveling
horizonLevelStrength = 0; horizonLevelStrength = 0;
@ -362,8 +361,9 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
static float previousSetpoint[3]; static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis]; const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity[axis]) if (ABS(currentVelocity) > maxVelocity[axis]) {
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis]; currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
}
previousSetpoint[axis] = currentPidSetpoint; previousSetpoint[axis] = currentPidSetpoint;
return currentPidSetpoint; return currentPidSetpoint;
@ -386,8 +386,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {