From 13122c29c0108d3b0df75622f29435a22e96bf95 Mon Sep 17 00:00:00 2001
From: William Freidank <37943506+teapfw@users.noreply.github.com>
Date: Mon, 21 Jul 2025 16:59:55 -0400
Subject: [PATCH] Fix simulated gimbal behavior with zero velocity (#25217)

* fix simulated gimbal behavior with zero velocity

* check for PX4_ISFINITE before checking for magnitude
---
 src/modules/simulation/gz_bridge/GZGimbal.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp
index 2d7ed9e069..451f846563 100644
--- a/src/modules/simulation/gz_bridge/GZGimbal.cpp
+++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp
@@ -240,6 +240,11 @@ float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp,
 {
 
 	if (PX4_ISFINITE(rate_stp)) {
+		if (math::abs_t(rate_stp) < FLT_EPSILON) {
+			// Handle zero velocity by sending the last target angle
+			return last_stp;
+		}
+
 		const float rate_diff = dt * rate_stp;
 		const float stp_from_rate = last_stp + rate_diff;