From 48816c922596ad996e53d4ebae7d2e166eb91c0e Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Sun, 4 Aug 2024 02:16:45 +0800
Subject: [PATCH 1/2] Fix wrong from std::abs(yaw) larger M_PI by vision.

---
 rm_gimbal_controllers/src/gimbal_base.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index be7b8c27..fdfd469a 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -301,6 +301,10 @@ void Controller::track(const ros::Time& time)
     ROS_WARN("%s", ex.what());
   }
   double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec());
+  while (yaw > M_PI)
+    yaw -= 2 * M_PI;
+  while (yaw < -M_PI)
+    yaw += 2 * M_PI;
   target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x;
   target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y;
   target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z;

From 34f6f5918b742fe6ef543424f5313481023a3b71 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Sun, 4 Aug 2024 02:16:49 +0800
Subject: [PATCH 2/2] Fix wrong.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index 1e5a9535..ce807a8c 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -12,8 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
-gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.0, 0.105, 0.5)
-gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.0, 0.105, 0.5)
+gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.105, 0.0, 0.5)
+gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5)
 gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
 gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
 gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)