From 87c3270b982f27338797da46ee278ea5ac7f73ed Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Thu, 4 Apr 2024 09:39:56 -0500
Subject: [PATCH] Start adding push message support

---
 include/message.h | 12 ++++++++++++
 include/sdk.h     | 30 ++++++++++++++++++++++++++++++
 src/connection.c  |  3 +++
 src/modules/sdk.c | 22 ++++++++++++++++++++++
 4 files changed, 67 insertions(+)

diff --git a/include/message.h b/include/message.h
index 00a6e09..08b5a6f 100644
--- a/include/message.h
+++ b/include/message.h
@@ -280,6 +280,18 @@ struct PACKED StreamCtrlResp {
     struct Footer footer;
 };
 
+struct PACKED AddSubMsgReq {
+    struct Header header;
+    uint8_t node_id;
+    uint8_t msg_id;
+    uint8_t reserved;
+    uint8_t sub_mode;
+    uint8_t count;
+    uint64_t subject_uuid;
+    uint16_t freq;
+    struct Footer footer;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
diff --git a/include/sdk.h b/include/sdk.h
index 9291b91..a5ce9aa 100644
--- a/include/sdk.h
+++ b/include/sdk.h
@@ -108,3 +108,33 @@ subscribe_add_node (
         uint16_t seq,
         bool ack );
 
+#define ADD_SUB_MSG_CMD 0x0348
+
+#define DDS_BATTERY 0x000200096862229f
+#define DDS_GIMBAL_BASE 0x00020009f5882874
+#define DDS_VELOCITY 0x0002000949a4009c
+#define DDS_ESC 0x00020009c14cb7c5
+#define DDS_ATTITUDE 0x000200096b986306
+#define DDS_IMU 0x00020009a7985b8d
+#define DDS_POSITION 0x00020009eeb7cece
+#define DDS_SA_STATUS 0x000200094a2c6d55
+#define DDS_CHASSIS_MODE 0x000200094fcb1146
+#define DDS_SBUS 0x0002000988223568
+#define DDS_SERVO 0x000200095f0059e7
+#define DDS_ARM 0x0002000926abd64d
+#define DDS_GRIPPER 0x00020009124d156a
+#define DDS_GIMBAL_POS 0x00020009f79b3c97
+#define DDS_STICK 0x0002000955e9a0fa
+#define DDS_MOVE_MODE 0x00020009784c7bfd
+#define DDS_TOF 0x0002000986e4c05a
+#define DDS_PINBOARD 0x00020009eebb9ffc
+
+void
+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/connection.c b/src/connection.c
index 9824b2b..066d73e 100644
--- a/src/connection.c
+++ b/src/connection.c
@@ -41,6 +41,8 @@ message_length(int cmd) {
             return sizeof(struct BlasterFireReq);
         case STREAM_CTRL_CMD:
             return sizeof(struct StreamCtrlReq);
+        case ADD_SUB_MSG_CMD:
+            return sizeof(struct AddSubMsgReq);
         default:
             return 0;
     }
@@ -58,6 +60,7 @@ message_module(int cmd) {
         case SET_ROBOT_MODE_CMD:
         case SUBNODE_RESET_CMD:
         case SUBSCRIBE_ADD_NODE_CMD:
+        case ADD_SUB_MSG_CMD:
             return host2byte(SDK_HOST, SDK_INDEX);
         case SET_WHEEL_SPEED_CMD:
         case CHASSIS_SPEED_MODE_CMD:
diff --git a/src/modules/sdk.c b/src/modules/sdk.c
index efef707..791c9a6 100644
--- a/src/modules/sdk.c
+++ b/src/modules/sdk.c
@@ -96,3 +96,25 @@ set_system_led (
     req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
 
 }
+
+void
+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.msg_id = msg_id;
+    req->sub.reserved = 0; // NOTE: Always 0
+    req->sub.mode = 0; // NOTE: Always 0
+    req->sub.subject_count = 1; // TODO: Add support for multiple
+    req->sub.subject_uuid = subject_uuid; // TODO: Add support for multiple
+    req->sub.freq = freq;
+
+    req_finalize(seq, ADD_SUB_MSG_CMD, ack, req);
+
+}