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

Merge 54172d1220 into acbab53d13

Browse source
This commit is contained in:
ChrisRosser 2025-07-11 16:43:58 +02:00 • committed by GitHub
commit e23b0783fc
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
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

@ -797,15 +797,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
static void rotateVector(float v[XYZ_AXIS_COUNT], const float rotation[XYZ_AXIS_COUNT]) static void rotateVector(float v[XYZ_AXIS_COUNT], const 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 // rotation in radians, all elements must be small
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { float pRatio = pidRuntime.pidCoefficient[FD_ROLL].Kp / pidRuntime.pidCoefficient[FD_PITCH].Kp;
int i_1 = (i + 1) % 3; float newV = v[FD_ROLL] + v[FD_PITCH] * rotation[FD_YAW] * pRatio;
int i_2 = (i + 2) % 3; v[FD_PITCH] -= v[FD_ROLL] * rotation[FD_YAW] / pRatio;
float newV = v[i_1] + v[i_2] * rotation[i]; v[FD_ROLL] = newV;
v[i_2] -= v[i_1] * rotation[i];
v[i_1] = newV;
}
} }
STATIC_UNIT_TESTED void rotateItermAndAxisError(void) STATIC_UNIT_TESTED void rotateItermAndAxisError(void)

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

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