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

Update to I-Term rotation

Browse source 
Update to I-Term rotation based on consideration of aerodynamics

Only rotate pitch and roll I terms based on Yaw Rate. Scale I term by P term ratio

Update pid_unittest.cc
This commit is contained in:
ChrisRosser 2024-03-22 16:48:06 +00:00
parent 7e0759076f
commit 54172d1220
2 changed files with 12 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -594,15 +594,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
{
// rotate v around rotation vector rotation
// rotate v around yaw component of vector rotation
// rotation in radians, all elements must be small
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
int i_1 = (i + 1) % 3;
int i_2 = (i + 2) % 3;
float newV = v[i_1] + v[i_2] * rotation[i];
v[i_2] -= v[i_1] * rotation[i];
v[i_1] = newV;
}
float pRatio = pidRuntime.pidCoefficient[FD_ROLL].Kp / pidRuntime.pidCoefficient[FD_PITCH].Kp;
float newV = v[FD_ROLL] + v[FD_PITCH] * rotation[FD_YAW] * pRatio;
v[FD_PITCH] -= v[FD_ROLL] * rotation[FD_YAW] / pRatio;
v[FD_ROLL] = newV;
}
STATIC_UNIT_TESTED void rotateItermAndAxisError(void)

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

@ -807,11 +807,11 @@ TEST(pidControllerTest, testItermRotationHandling)
pidData[FD_ROLL].I = 10;
pidData[FD_PITCH].I = 1000;
pidData[FD_YAW].I = 1000;
gyro.gyroADCf[FD_ROLL] = -1000;
gyro.gyroADCf[FD_YAW] = -1000;
rotateItermAndAxisError();
EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
EXPECT_NEAR(-86.294029235839844, pidData[FD_ROLL].I, calculateTolerance(-86.294029235839844));
EXPECT_NEAR(1001.396240234375, pidData[FD_PITCH].I, calculateTolerance(1001.396240234375));
EXPECT_FLOAT_EQ(1000, pidData[FD_YAW].I);
pidProfile->abs_control_gain = 10;
pidInit(pidProfile);
@ -824,9 +824,9 @@ TEST(pidControllerTest, testItermRotationHandling)
axisError[FD_PITCH] = 1000;
axisError[FD_YAW] = 1000;
rotateItermAndAxisError();
EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10);
EXPECT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
EXPECT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
EXPECT_NEAR(-86.294029235839844, pidData[FD_ROLL].I, calculateTolerance(-86.294029235839844));
EXPECT_NEAR(1001.396240234375, pidData[FD_PITCH].I, calculateTolerance(1001.396240234375));
EXPECT_FLOAT_EQ(1000, pidData[FD_YAW].I);
}
TEST(pidControllerTest, testLaunchControl)