From 59704c5e9dd8319f9c65f52c9258f4f9243600cf Mon Sep 17 00:00:00 2001
From: Florian Vahl <git@flova.de>
Date: Sun, 5 Nov 2023 18:35:06 +0000
Subject: [PATCH] Adapt to new gsdk version

---
 gremsy_base/config/config.yaml | 6 +++---
 gremsy_base/src/gSDK           | 2 +-
 gremsy_base/src/ros_gremsy.cpp | 5 ++++-
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/gremsy_base/config/config.yaml b/gremsy_base/config/config.yaml
index 0564c18..c7a514d 100644
--- a/gremsy_base/config/config.yaml
+++ b/gremsy_base/config/config.yaml
@@ -1,7 +1,7 @@
 # Config
 device: "/dev/ttyUSB0"
 baudrate: 115200
-state_poll_rate: 10.0
-goal_push_rate: 60.0
-gimbal_mode: 1
+state_poll_rate: 5.0
+goal_push_rate: 10.0
+gimbal_mode: 0
 
diff --git a/gremsy_base/src/gSDK b/gremsy_base/src/gSDK
index fdc56b4..9df7a43 160000
--- a/gremsy_base/src/gSDK
+++ b/gremsy_base/src/gSDK
@@ -1 +1 @@
-Subproject commit fdc56b48f7a23fb98865e9ddbb4f534bf3943a19
+Subproject commit 9df7a43c461b5152cd4cefecadbcb840d66e039c
diff --git a/gremsy_base/src/ros_gremsy.cpp b/gremsy_base/src/ros_gremsy.cpp
index 2d5d7c7..f20a85c 100644
--- a/gremsy_base/src/ros_gremsy.cpp
+++ b/gremsy_base/src/ros_gremsy.cpp
@@ -87,7 +87,10 @@ void GimbalNode::gimbalStateTimerCallback(const ros::TimerEvent& event)
     encoder_ros_msg.vector.y = ((float) encoder_values.pitch) * DEG_TO_RAD;
     encoder_ros_msg.vector.z = ((float) encoder_values.yaw) * DEG_TO_RAD;
 
-    encoder_pub.publish(encoder_ros_msg); 
+    if (encoder_values.roll or encoder_values.pitch or encoder_values.yaw)
+    {
+        encoder_pub.publish(encoder_ros_msg); 
+    }
 
     //// Publish Gimbal IMU (Currently this deadlocks)
     //Gimbal_Interface::imu_t imu_data = gimbal_interface_->get_gimbal_raw_imu();