From 23a07f99d955acfacfb2a94b4e66bf2a1b24b6ce Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Thu, 25 Apr 2024 09:52:58 -0500
Subject: [PATCH] Add robot position push topic

---
 include/message.h | 15 +++++++++++++++
 include/sdk.h     |  2 ++
 src/robo.c        | 24 +++++++++++++++---------
 3 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/include/message.h b/include/message.h
index 90e29f5..b9764d8 100644
--- a/include/message.h
+++ b/include/message.h
@@ -302,6 +302,20 @@ struct PACKED AddSubMsgResp {
     struct Footer footer;
 };
 
+struct PACKED PushPeriodMsg {
+    struct Header header;
+    uint8_t sub_mode;
+    uint8_t msg_id;
+    union {
+        struct {
+            float x;
+            float y;
+            float z;
+        } position;
+    };
+    struct Footer footer;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -335,6 +349,7 @@ union Response {
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
     struct AddSubMsgResp sub;
+    struct PushPeriodMsg push;
 };
 union Message {
     struct Header header;
diff --git a/include/sdk.h b/include/sdk.h
index fe9f7b1..64d1508 100644
--- a/include/sdk.h
+++ b/include/sdk.h
@@ -110,6 +110,8 @@ subscribe_add_node (
 
 #define ADD_SUB_MSG_CMD 0x0348
 
+#define PUSH_PERIOD_MSG_CMD 0x0848
+
 #define DDS_BATTERY 0x000200096862229f
 #define DDS_GIMBAL_BASE 0x00020009f5882874
 #define DDS_VELOCITY 0x0002000949a4009c
diff --git a/src/robo.c b/src/robo.c
index 84e68fc..a201634 100644
--- a/src/robo.c
+++ b/src/robo.c
@@ -28,6 +28,8 @@ struct RobotImp {
 
     bool sdk_mode;
 
+    float x, y, z;
+
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -36,7 +38,7 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
-        SUBSCRIBING_GIMBAL_STATUS,
+        SUBSCRIBING_POSITION_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -171,10 +173,10 @@ robot_work(Robot robot) {
             break;
         }
 
-        case SUBSCRIBING_GIMBAL_STATUS:
+        case SUBSCRIBING_POSITION_STATUS:
         {
-            add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_BASE );
-            req_send(robot->client->sdk_conn, &req);
+            add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
+            req_send(robot->client->dev_conn, &req);
             robot->state = WAITING;
             break;
         }
@@ -253,6 +255,8 @@ robot_work(Robot robot) {
     switch(resp.header.cmd) {
         case 0:
             break;
+        default:
+            break;
         case SET_SDK_CONNECTION_CMD:
             // TODO: Do more with this
             if(resp.resp.sdkconn.retcode) {
@@ -288,17 +292,13 @@ robot_work(Robot robot) {
                 robot->state = STOPPED;
                 break;
             }
-            robot->state = SUBSCRIBING_GIMBAL_STATUS;
+            robot->state = SUBSCRIBING_POSITION_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:
@@ -308,6 +308,12 @@ robot_work(Robot robot) {
             }
             robot->state = READY;
             break;
+        case PUSH_PERIOD_MSG_CMD:
+            // assume it is a position status for now
+            robot->x = resp.resp.push.position.x;
+            robot->y = resp.resp.push.position.y;
+            robot->z = resp.resp.push.position.z;
+            break;
     }
 
     return 1;