Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Merge pull request #6432 from joelucid/integrated_yaw

Browse source 
For discussion: Integrated yaw control
This commit is contained in:
Michael Keller 2018-12-15 18:40:00 +13:00 • committed by GitHub
commit 60118da63f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 30 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -179,6 +179,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.launchControlAngleLimit = 0,
.launchControlGain = 40,
.launchControlAllowTriggerReset = true,
.use_integrated_yaw = false,
.integrated_yaw_relax = 200,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150;
@ -448,6 +450,11 @@ static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
static FAST_RAM_ZERO_INIT float launchControlKi;
#endif
#ifdef USE_INTEGRATED_YAW_CONTROL
static FAST_RAM_ZERO_INIT bool useIntegratedYaw;
static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax;
#endif
void pidResetIterm(void)
{
for (int axis = 0; axis < 3; axis++) {
@ -585,6 +592,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
}
launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
#endif
#ifdef USE_INTEGRATED_YAW_CONTROL
useIntegratedYaw = pidProfile->use_integrated_yaw;
integratedYawRelax = pidProfile->integrated_yaw_relax;
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -1244,7 +1256,16 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif
// calculating the PID sum
pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
#ifdef USE_INTEGRATED_YAW_CONTROL
if (axis == FD_YAW && useIntegratedYaw) {
pidData[axis].Sum += pidSum * dT * 100.0f;
pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f;
} else
#endif
{
pidData[axis].Sum = pidSum;
}
}
// Disable PID control if at zero throttle or if gyro overflow detected