From 83d91c148950ad9e71b774156283c8a436c79745 Mon Sep 17 00:00:00 2001
From: PerFrivik <per.a.frivik@gmail.com>
Date: Mon, 25 Mar 2024 10:58:35 +0100
Subject: [PATCH] Differential Drive: Improved logic, to be independent of the
 heading gain

---
 .../DifferentialDriveGuidance/DifferentialDriveGuidance.cpp  | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
index 02c9a2127a..66aed83ced 100644
--- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
+++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
@@ -86,10 +86,11 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 			}
 
 			float minTurningSpeed = _param_rdd_min_turning_speed.get();
+			float headingPGain = _param_rdd_p_gain_heading.get();
 
 			// Make sure we do not get stuck while turning as the error gets too small
-			if (fabsf(_heading_error) < minTurningSpeed) {
-				_heading_error = (_heading_error > 0) ? minTurningSpeed : -minTurningSpeed;
+			if ((fabsf(_heading_error) < minTurningSpeed) && (headingPGain > 0.01f)) {
+				_heading_error = (_heading_error > 0) ? minTurningSpeed * 1 / headingPGain : -minTurningSpeed * 1 / headingPGain;
 			}
 
 			break;