From 02aed35c6eb65cb772c44e005690d1142998aa9c Mon Sep 17 00:00:00 2001 From: Eli Barnett Date: Sun, 18 Feb 2024 00:12:00 -0500 Subject: [PATCH] [sysid] Relax peak acceleration search (#6378) --- .../src/main/native/cpp/analysis/FilteringUtils.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp index db0381103a..8432e7c8ff 100644 --- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector* data, wpi::sgn(b.velocity) * b.acceleration; }); + // Current limiting can delay onset of the peak acceleration, so we need to + // find the first acceleration *near* the max. Magic number tolerance here + // because this whole file is tech debt already + auto accelBegins = std::find_if( + data->begin(), data->end(), [&maxAccel](const auto& measurement) { + return wpi::sgn(measurement.velocity) * measurement.acceleration > + 0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration; + }); + units::second_t velocityDelay; - if (maxAccel != data->end()) { - velocityDelay = maxAccel->timestamp - firstTimestamp; + if (accelBegins != data->end()) { + velocityDelay = accelBegins->timestamp - firstTimestamp; // Trim data before max acceleration data->erase(data->begin(), maxAccel);