diff --git a/src/lib/stick_yaw/StickYaw.cpp b/src/lib/stick_yaw/StickYaw.cpp index e0c2582fb5a..9f7815ad23c 100644 --- a/src/lib/stick_yaw/StickYaw.cpp +++ b/src/lib/stick_yaw/StickYaw.cpp @@ -43,6 +43,9 @@ StickYaw::StickYaw(ModuleParams *parent) : void StickYaw::reset(const float yaw, const float unaided_yaw) { + // The filter state outlives deactivation; a reset means there is no stick history to carry over. + _yawspeed_filter.reset(0.f); + if (PX4_ISFINITE(unaided_yaw)) { _yaw_error_lpf.reset(wrap_pi(yaw - unaided_yaw)); } diff --git a/src/lib/stick_yaw/StickYawTest.cpp b/src/lib/stick_yaw/StickYawTest.cpp index 0af5bca3d48..9f3918b51d1 100644 --- a/src/lib/stick_yaw/StickYawTest.cpp +++ b/src/lib/stick_yaw/StickYawTest.cpp @@ -36,6 +36,30 @@ #include +TEST(StickYawTest, ResetClearsYawspeedFilter) +{ + param_control_autosave(false); + + StickYaw stick_yaw{nullptr}; + float yawspeed_sp = 0.f; + float yaw_sp = NAN; + const float dt = 0.01f; + + // GIVEN: the filter charged up by a sustained full-deflection yaw stick + for (int i = 0; i < 500; i++) { + stick_yaw.generateYawSetpoint(yawspeed_sp, yaw_sp, 1.f, 0.f, dt); + } + + EXPECT_GT(yawspeed_sp, 0.1f); + + // WHEN: reset with no unaided yaw available, then the centred stick is sampled + stick_yaw.reset(0.f); + stick_yaw.generateYawSetpoint(yawspeed_sp, yaw_sp, 0.f, 0.f, dt); + + // THEN: no yawspeed is commanded + EXPECT_EQ(yawspeed_sp, 0.f); +} + TEST(StickYawTest, UnaidedYawNanTransitionNoYawJump) { // When unaided_yaw transitions from finite to NAN mid-flight,