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

Refactoring of IMU and ACC

Browse source
This commit is contained in:
Štěpán Dalecký 2022-01-20 20:37:23 +01:00
parent b6b24ba946
commit f85ebba6a4
24 changed files with 141 additions and 195 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -375,39 +375,39 @@ TEST(pidControllerTest, testPidLevel) {
float currentPidSetpoint = 30;
rollAndPitchTrims_t angleTrim = { { 0, 0 } };
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
// Test attitude response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(275, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-275, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(244.07211, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-244.07211, currentPidSetpoint);
setStickPosition(FD_ROLL, -0.5f);
setStickPosition(FD_PITCH, 0.5f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(137.5, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-93.487915, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(93.487915, currentPidSetpoint);
attitude.values.roll = -275;
attitude.values.pitch = 275;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-12.047981, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
// Disable ANGLE_MODE
disableFlightMode(ANGLE_MODE);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(11.07958, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
// Test level mode expo
enableFlightMode(ANGLE_MODE);
@ -417,10 +417,10 @@ TEST(pidControllerTest, testPidLevel) {
setStickPosition(FD_PITCH, -0.5f);
currentControlRateProfile->levelExpo[FD_ROLL] = 50;
currentControlRateProfile->levelExpo[FD_PITCH] = 26;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(85.9375, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-110.6875, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(76.208672, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-98.175163, currentPidSetpoint);
}
@ -433,18 +433,18 @@ TEST(pidControllerTest, testPidHorizon) {
// Test full stick response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
// Expect full rate output on full stick
// Test zero stick response
setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0);
EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(1.0f, calcHorizonLevelStrength());
// Test small stick response
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82));
EXPECT_NEAR(0.811f, calcHorizonLevelStrength(), calculateTolerance(0.82));
}
TEST(pidControllerTest, testMixerSaturation) {