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

Merge pull request #9569 from ctzsnooze/iterm-windup-setting-yaw-only

Browse source 
apply iterm_windup to yaw only
This commit is contained in:
Michael Keller 2020-03-15 13:52:18 +13:00 • committed by GitHub
commit 04d260d620
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 24 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1454,11 +1454,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate I component
#ifdef USE_LAUNCH_CONTROL
// if launch control is active override the iterm gains
const float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
#else
const float Ki = pidCoefficient[axis].Ki;
float Ki = pidCoefficient[axis].Ki;
#endif
pidData[axis].I = constrainf(previousIterm + (Ki * dynCi + agGain) * itermErrorRate, -itermLimit, itermLimit);
// iterm_windup on yaw only
if (axis == FD_YAW) {
Ki *= dynCi;
}
pidData[axis].I = constrainf(previousIterm + (Ki + agGain) * itermErrorRate, -itermLimit, itermLimit);
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;

34
src/test/unit/pid_unittest.cc
View file

@ -255,7 +255,7 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_NEAR(-7.8, pidData[FD_ROLL].I, calculateTolerance(-7.8));
EXPECT_NEAR(-150, pidData[FD_ROLL].I, calculateTolerance(-150));
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
EXPECT_NEAR(-198.4, pidData[FD_ROLL].D, calculateTolerance(-198.4));
@ -270,8 +270,8 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
EXPECT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_NEAR(-15.6, pidData[FD_ROLL].I, calculateTolerance(-15.6));
EXPECT_NEAR(9.8, pidData[FD_PITCH].I, calculateTolerance(9.8));
EXPECT_NEAR(-150, pidData[FD_ROLL].I, calculateTolerance(-150));
EXPECT_NEAR(150, pidData[FD_PITCH].I, calculateTolerance(150));
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_NEAR(231.4, pidData[FD_PITCH].D, calculateTolerance(231.4));
@ -285,8 +285,8 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
EXPECT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
EXPECT_NEAR(-224.2, pidData[FD_YAW].P, calculateTolerance(-224.2));
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
EXPECT_NEAR(-150, pidData[FD_ROLL].I, calculateTolerance(-150));
EXPECT_NEAR(150, pidData[FD_PITCH].I, calculateTolerance(150));
EXPECT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
@ -295,8 +295,8 @@ TEST(pidControllerTest, testPidLoop) {
// Simulate Iterm behaviour during mixer saturation
simulatedMotorMixRange = 1.2f;
pidController(pidProfile, currentTestTime());
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
EXPECT_NEAR(-150, pidData[FD_ROLL].I, calculateTolerance(-150));
EXPECT_NEAR(150, pidData[FD_PITCH].I, calculateTolerance(150));
EXPECT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
simulatedMotorMixRange = 0;
@ -312,8 +312,8 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
EXPECT_NEAR(-150, pidData[FD_ROLL].I, calculateTolerance(-150));
EXPECT_NEAR(150, pidData[FD_PITCH].I, calculateTolerance(150));
EXPECT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
@ -418,8 +418,8 @@ TEST(pidControllerTest, testMixerSaturation) {
pidController(pidProfile, currentTestTime());
// Expect no iterm accumulation
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(150, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(-150, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
// Test itermWindup limit
@ -445,7 +445,7 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f;
pidController(pidProfile, currentTestTime());
EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm);
EXPECT_LE(pidData[FD_ROLL].I, rollTestIterm);
EXPECT_GE(pidData[FD_PITCH].I, pitchTestIterm);
EXPECT_LT(pidData[FD_YAW].I, yawTestIterm);
}
@ -724,9 +724,9 @@ TEST(pidControllerTest, testLaunchControl) {
pidController(pidProfile, currentTestTime());
EXPECT_NEAR(25.62, pidData[FD_ROLL].P, calculateTolerance(25.62));
EXPECT_NEAR(1.56, pidData[FD_ROLL].I, calculateTolerance(1.56));
EXPECT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150));
EXPECT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
EXPECT_NEAR(-1.56, pidData[FD_PITCH].I, calculateTolerance(-1.56));
EXPECT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150));
EXPECT_NEAR(44.84, pidData[FD_YAW].P, calculateTolerance(44.84));
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
@ -755,7 +755,7 @@ TEST(pidControllerTest, testLaunchControl) {
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_NEAR(37.15, pidData[FD_PITCH].P, calculateTolerance(37.15));
EXPECT_NEAR(1.56, pidData[FD_PITCH].I, calculateTolerance(1.56));
EXPECT_NEAR(150, pidData[FD_PITCH].I, calculateTolerance(150));
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
@ -774,9 +774,9 @@ TEST(pidControllerTest, testLaunchControl) {
pidController(pidProfile, currentTestTime());
EXPECT_NEAR(25.62, pidData[FD_ROLL].P, calculateTolerance(25.62));
EXPECT_NEAR(1.56, pidData[FD_ROLL].I, calculateTolerance(1.56));
EXPECT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150));
EXPECT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
EXPECT_NEAR(-1.56, pidData[FD_PITCH].I, calculateTolerance(-1.56));
EXPECT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150));
EXPECT_NEAR(44.84, pidData[FD_YAW].P, calculateTolerance(44.84));
EXPECT_NEAR(1.56, pidData[FD_YAW].I, calculateTolerance(1.56));
}