fix(stick_yaw): clear the yawspeed filter on reset

This prevents a stale yawspeed setpoint from being released when the module
is activated again with the stick centred
This commit is contained in:
bresch
2026-09-29 10:36:45 +02:00
committed by Matthias Grob
parent 3a2baa472f
commit d6817b8fdf
2 changed files with 27 additions and 0 deletions
+3
View File
@@ -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));
}
+24
View File
@@ -36,6 +36,30 @@
#include <px4_platform_common/defines.h>
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,