From 8811482f1df86a448075839530cdbedf385b5d1d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 19 Jan 2022 10:40:06 +0100 Subject: [PATCH] PositionControlTest: add integrator windup with invalid setpoint combination unit test This bug was by chance found during simulation testing and debugging. The unit test is to easily reproduce and cover this case. --- .../PositionControl/PositionControlTest.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 5afc32dde1..5e625d0878 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -399,3 +399,23 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust) // the output is still the same EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); } + +TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) +{ + // GIVEN: the controller was ran with an invalid setpoint containing some valid values + _input_setpoint.x = .1f; + _input_setpoint.y = .2f; + // all z-axis setpoints stay NAN + EXPECT_FALSE(runController()); + + // WHEN: we run the controller with a valid setpoint + resetInputSetpoint(); + _input_setpoint.vx = 0.f; + _input_setpoint.vy = 0.f; + _input_setpoint.vz = 0.f; + EXPECT_TRUE(runController()); + + // THEN: the integral did not wind up and produce unexpected deviation + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +}