fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 02:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

AttitudeControlTest: add scale testing for yaw weight

Browse Source
This commit is contained in:
Matthias Grob 2020-03-25 15:42:25 +01:00
parent 96bbc63eb1
commit b8576cccc8
1 changed files with 28 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
View File

@ -49,14 +49,14 @@ class AttitudeControlConvergenceTest : public ::testing::Test
public:
AttitudeControlConvergenceTest()
{
_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f));
_attitude_control.setRateLimit(Vector3f(100, 100, 100));
_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f);
_attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
}
void checkConvergence()
{
int i; // need function scope to check how many steps
Vector3f rate_setpoint(1000, 1000, 1000);
Vector3f rate_setpoint(1000.f, 1000.f, 1000.f);
for (i = 100; i > 0; i--) {
// run attitude control to get rate setpoints
@ -109,3 +109,28 @@ TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence)
}
}
}
TEST(AttitudeControlTest, YawWeightScaling)
{
// GIVEN: default tuning
AttitudeControl attitude_control;
const float yaw_gain = 2.8f;
const float yaw_sp = .1f;
attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f);
attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f));
// WHEN: we command a pure yaw turn
Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
// THEN: no actuation in roll, pitch
EXPECT_EQ(Vector2f(rate_setpoint), Vector2f());
// THEN: actuation error * gain in yaw
EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f);
// GIVEN: corner case of zero yaw weight
attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f);
// WHEN: we command a pure yaw turn
rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f);
// THEN: no actuation (also no NAN)
EXPECT_EQ(rate_setpoint, Vector3f());
}