mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-10-06 09:02:52 +08:00
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:
@@ -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));
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user