From 60cc6302462cf2c75397f1de4d1d1281a01cfbc5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 26 Nov 2024 19:37:00 +0000 Subject: [PATCH] Calc dt only once --- src/pid.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pid.cpp b/src/pid.cpp index 10d94ab..f6b7824 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -130,10 +130,11 @@ double Pid::computeCommand(double error, uint64_t dt_ns) error_dot_ = d_error_; // Calculate the derivative error - error_dot_ = (error - p_error_last_) / (static_cast(dt_ns) / 1e9); + double dt_s = (static_cast(dt_ns) / 1e9); + error_dot_ = (error - p_error_last_) / dt_s; p_error_last_ = error; - return computeCommand(error, error_dot_, dt_ns); + return computeCommand(error, error_dot_, dt_s); } double Pid::computeCommand(double error, double dt_s)