include/message.h

@@ -296,23 +296,9 @@ struct PACKED AddSubMsgResp {
     struct Header header;
     uint8_t retcode;
     uint8_t pub_node_id;
-    uint8_t ack_sub_mode;
+    uint8_t ack_node_id;
     uint8_t ack_msg_id;
-    uint16_t ack_err_uid_data;
+    uint32_t ack_err_uid_data;
-    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;
 };
 
@@ -349,7 +335,6 @@ union Response {
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
     struct AddSubMsgResp sub;
-    struct PushPeriodMsg push;
 };
 union Message {
     struct Header header;

include/sdk.h

@@ -110,8 +110,6 @@ 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
@@ -136,6 +134,7 @@ 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 );

src/modules/sdk.c

@@ -102,11 +102,12 @@ 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 = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req->sub.node_id = node_id;
     req->sub.msg_id = msg_id;
     req->sub.reserved = 0; // NOTE: Always 0
     req->sub.sub_mode = 0; // NOTE: Always 0

src/robo.c

@@ -28,8 +28,6 @@ struct RobotImp {
 
     bool sdk_mode;
 
-    float x, y, z;
-
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -38,7 +36,6 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
-        SUBSCRIBING_POSITION_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -173,14 +170,6 @@ robot_work(Robot robot) {
             break;
         }
 
-        case SUBSCRIBING_POSITION_STATUS:
-        {
-            add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
-            req_send(robot->client->dev_conn, &req);
-            robot->state = WAITING;
-            break;
-        }
-
         case READY:
         {
             if(robot->dirty_wheels) {
@@ -255,8 +244,6 @@ 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) {
@@ -292,13 +279,6 @@ robot_work(Robot robot) {
                 robot->state = STOPPED;
                 break;
             }
-            robot->state = SUBSCRIBING_POSITION_STATUS;
-            break;
-        case ADD_SUB_MSG_CMD:
-            if(resp.resp.sub.retcode) {
-                robot->state = STOPPED;
-                break;
-            }
             robot->state = SENDING_HEARTBEAT;
             break;
         case SDK_HEARTBEAT_CMD:
@@ -308,12 +288,6 @@ 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;