From 21980c744713175a282b41544c617921ff67e773 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 31 Oct 2024 20:37:15 -0700
Subject: [PATCH] [sysid] Clamp feedback measurement delay to zero or higher
 (#7319)

LQR latency compensation applies exponential decay to the feedback
gains, so a negative latency causes them to exponentially grow.
---
 sysid/src/main/native/cpp/view/Analyzer.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp
index 748a1e358c..03b39171b2 100644
--- a/sysid/src/main/native/cpp/view/Analyzer.cpp
+++ b/sysid/src/main/native/cpp/view/Analyzer.cpp
@@ -53,8 +53,9 @@ void Analyzer::UpdateFeedforwardGains() {
     m_accelRMSE = feedforwardGains.olsResult.rmse;
     m_settings.preset.measurementDelay =
         m_settings.type == FeedbackControllerLoopType::kPosition
-            ? m_manager->GetPositionDelay()
-            : m_manager->GetVelocityDelay();
+            // Clamp feedback measurement delay to ≥ 0
+            ? units::math::max(0_s, m_manager->GetPositionDelay())
+            : units::math::max(0_s, m_manager->GetVelocityDelay());
     PrepareGraphs();
   } catch (const sysid::InvalidDataError& e) {
     m_state = AnalyzerState::kGeneralDataError;