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

Fix bug with antiwindup inversion

Browse source
This commit is contained in:
borisbstyle 2018-09-10 00:07:55 +02:00
parent 39ced6bbfe
commit 864a2d6a2b
2 changed files with 12 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -857,8 +857,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
} }
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
// gradually scale back integration when above windup point // gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; const float dynCi = constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f)
* dT * itermAccelerator;
// Precalculate gyro deta for D-term here, this allows loop unrolling // Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[XYZ_AXIS_COUNT]; float gyroRateDterm[XYZ_AXIS_COUNT];

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

@ -269,6 +269,14 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25)); ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25));
// Simulate Iterm behaviour during mixer saturation
simulatedMotorMixRange = 1.2f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
simulatedMotorMixRange = 0;
// Match the stick to gyro to stop error // Match the stick to gyro to stop error
simulatedSetpointRate[FD_ROLL] = 100; simulatedSetpointRate[FD_ROLL] = 100;
simulatedSetpointRate[FD_PITCH] = -100; simulatedSetpointRate[FD_PITCH] = -100;
@ -277,14 +285,13 @@ TEST(pidControllerTest, testPidLoop) {
for(int loop = 0; loop < 5; loop++) { for(int loop = 0; loop < 5; loop++) {
pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
} }
// Iterm is stalled as it is not accumulating anymore // Iterm is stalled as it is not accumulating anymore
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5)); ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6)); ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.5)); ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);