From 0340d40b847bc99ec36a3b2227cf79f4188bc19b Mon Sep 17 00:00:00 2001
From: dev01 <dev01@example.com>
Date: Wed, 3 Jun 2026 22:08:14 +0800
Subject: [PATCH] fix: gate gimbal callbacks by current mode

Add _current_mode state to prevent manual commands during auto
tracking and vice versa. Remove unused auto_tracking_enabled param.
---
 .../gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
index 1547bc9..1bf7225 100644
--- a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
+++ b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
@@ -25,8 +25,6 @@ class GimbalNode(Node):
 
         self.declare_parameter('serial_port', '/dev/ttyTHS1')
         self.declare_parameter('baud_rate', 115200)
-        self.declare_parameter('auto_tracking_enabled', True)
-
         port = self.get_parameter('serial_port').value
         baud = self.get_parameter('baud_rate').value
 
@@ -53,6 +51,8 @@ class GimbalNode(Node):
             String, '/gimbal/mode', self._mode_callback, 10
         )
 
+        self._current_mode = 'debug'
+
         self.get_logger().info(
             'GimbalNode ready - subs: /cmd_gimbal, /vision/target, '
             '/pid_config, /gimbal/mode'
@@ -66,6 +66,8 @@ class GimbalNode(Node):
             self.get_logger().error(f'Serial write error: {e}')
 
     def _cmd_gimbal_callback(self, msg: Twist):
+        if self._current_mode != 'manual':
+            return
         yaw_vel = float(msg.angular.z)
         pitch_vel = float(msg.angular.y)
         self.get_logger().debug(
@@ -79,6 +81,8 @@ class GimbalNode(Node):
         self._write_frame(data)
 
     def _target_callback(self, msg: Point):
+        if self._current_mode != 'auto':
+            return
         self.get_logger().debug(
             f'Auto tracking: error_x={msg.x:.4f}, error_y={msg.y:.4f}'
         )
@@ -138,6 +142,7 @@ class GimbalNode(Node):
             self.get_logger().warn(f'Unknown mode command: {mode}')
             return
 
+        self._current_mode = mode
         self.get_logger().info(f'Sending command: 0xA7 cmd=0x{cmd:02X} ({mode})')
         data = bytearray([0xA7, cmd])
         data.append(_checksum(data))