diff --git a/include/sdk.h b/include/sdk.h
index a5ce9aa..fe9f7b1 100644
--- a/include/sdk.h
+++ b/include/sdk.h
@@ -134,7 +134,6 @@ add_sub_msg (
         union Request* req,
         uint16_t seq,
         bool ack,
-        uint8_t node_id,
         uint8_t msg_id,
         uint8_t freq,
         uint64_t subject_uuid );
diff --git a/src/modules/sdk.c b/src/modules/sdk.c
index 31d1e25..46f3c22 100644
--- a/src/modules/sdk.c
+++ b/src/modules/sdk.c
@@ -102,12 +102,11 @@ add_sub_msg (
         union Request* req,
         uint16_t seq,
         bool ack,
-        uint8_t node_id,
         uint8_t msg_id,
         uint8_t freq,
         uint64_t subject_uuid ) {
 
-    req->sub.node_id = node_id;
+    req->sub.node_id = host2byte(CLIENT_HOST, CLIENT_INDEX);
     req->sub.msg_id = msg_id;
     req->sub.reserved = 0; // NOTE: Always 0
     req->sub.sub_mode = 0; // NOTE: Always 0
diff --git a/src/robo.c b/src/robo.c
index 2a806c2..84e68fc 100644
--- a/src/robo.c
+++ b/src/robo.c
@@ -36,6 +36,7 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
+        SUBSCRIBING_GIMBAL_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -170,6 +171,14 @@ robot_work(Robot robot) {
             break;
         }
 
+        case SUBSCRIBING_GIMBAL_STATUS:
+        {
+            add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_BASE );
+            req_send(robot->client->sdk_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
         case READY:
         {
             if(robot->dirty_wheels) {
@@ -279,6 +288,17 @@ robot_work(Robot robot) {
                 robot->state = STOPPED;
                 break;
             }
+            robot->state = SUBSCRIBING_GIMBAL_STATUS;
+            break;
+        case ADD_SUB_MSG_CMD:
+            if(resp.resp.sub.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            if(resp.resp.sub.ack_node_id != host2byte(CLIENT_HOST, CLIENT_INDEX)) {
+                robot->state = STOPPED;
+                break;
+            }
             robot->state = SENDING_HEARTBEAT;
             break;
         case SDK_HEARTBEAT_CMD: