Add pad deadzone

husarion · Dec 4, 2024 · 2f2a297 · 2f2a297
1 parent 91c04f8
commit 2f2a297
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf_teleop_node.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf_teleop_node.py
@@ -38,6 +38,8 @@
 LINK_QUALITY_VERY_LOW_THRESHOLD = 15
 LINK_QUALITY_LOW_THRESHOLD = 30
 
+PAD_DEADZONE = 0.02
+
 
 # RC remote to channel id mapping
 class Switch(IntEnum):
@@ -194,6 +196,11 @@ def handle_message(self, msg: CRSFMessage):
                 t.linear.x = channels[Switch.RIGHT_HORIZONTAL] * linear_speed_modifier
                 t.angular.z = -channels[Switch.LEFT_VERTICAL] * angular_speed_modifier
 
+                if abs(t.linear.x) < PAD_DEADZONE:
+                    t.linear.x = 0
+                if abs(t.angular.z) < PAD_DEADZONE:
+                    t.angular.z = 0
+
                 self.cmd_vel_publisher.publish(t)
 
         elif msg.msg_type == PacketType.LINK_STATISTICS: