From d63bbbb164dd3285f3ea8f22a568fdcd381da612 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 6 Oct 2022 15:15:12 +0200 Subject: filter/accela: fix accidentally clamping position data Introduced in: 7252c001 Tested by: @Pieloth Issue: #1349 --- filter-accela/ftnoir_filter_accela.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp index d3464f7b..940ac50c 100644 --- a/filter-accela/ftnoir_filter_accela.cpp +++ b/filter-accela/ftnoir_filter_accela.cpp @@ -154,7 +154,8 @@ void accela::filter(const double* input, double *output) { output[k] *= dt; output[k] += last_output[k]; - if (fabs(output[k]) > half_turn) output[k] -= copysign(full_turn, output[k]); + if (k >= Yaw && fabs(output[k]) > half_turn) + output[k] -= copysign(full_turn, output[k]); last_output[k] = output[k]; } -- cgit v1.2.3