From f0befa0b7bf1860a8d29385709706e81647fe94a Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Mon, 2 Jan 2023 11:42:54 -0600
Subject: [PATCH] Add heartbeat message

---
 include/message.h        | 13 +++++++++++++
 include/sdk_connection.h |  6 ++++++
 src/messages/sdk_mode.c  |  8 ++++++++
 src/robomastersh.c       | 12 +++++++-----
 4 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/include/message.h b/include/message.h
index 92b8934..76ebe18 100644
--- a/include/message.h
+++ b/include/message.h
@@ -178,6 +178,17 @@ struct PACKED SetWheelSpeedResp
     struct Footer footer;
 };
 
+struct PACKED SdkHeartbeatReq {
+    struct Header header;
+    struct Footer footer;
+};
+
+struct PACKED SdkHeartbeatResp {
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
 struct PACKED SetSdkModeReq {
     struct Header header;
     uint8_t enable;
@@ -213,6 +224,7 @@ union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
     struct SetSdkModeReq sdkmode;
+    struct SdkHeartbeatReq heartbeat;
     struct SetSystemLedReq led;
     struct SetWheelSpeedReq wheel;
     struct SetChassisWheelSpeedReq chswheel;
@@ -225,6 +237,7 @@ union Response {
     struct Header header;
     struct SetSdkConnectionResp sdkconn;
     struct SetSdkModeResp sdkmode;
+    struct SdkHeartbeatResp heartbeat;
     struct SetSystemLedResp led;
     struct SetWheelSpeedResp wheel;
     struct SetChassisWheelSpeedResp chswheel;
diff --git a/include/sdk_connection.h b/include/sdk_connection.h
index e755332..2bf95e7 100644
--- a/include/sdk_connection.h
+++ b/include/sdk_connection.h
@@ -19,3 +19,9 @@ set_sdk_connection(
 		enum CONNECTION connection_type,
 		uint32_t ip_address,
 		uint16_t port );
+
+static const uint8_t SDK_HEARTBEAT_CMDID = 0xD5;
+
+void
+sdk_heartbeat(
+		Client session );
diff --git a/src/messages/sdk_mode.c b/src/messages/sdk_mode.c
index 1f4ef6b..673fc30 100644
--- a/src/messages/sdk_mode.c
+++ b/src/messages/sdk_mode.c
@@ -11,3 +11,11 @@ set_sdk_mode(
     req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetSdkModeReq), &req);
     req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq));
 }
+
+void
+sdk_heartbeat(
+		Client session ) {
+    union Request req = {0};
+    req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SdkHeartbeatReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct SdkHeartbeatReq));
+}
diff --git a/src/robomastersh.c b/src/robomastersh.c
index 2272b5e..33fdd41 100644
--- a/src/robomastersh.c
+++ b/src/robomastersh.c
@@ -44,6 +44,13 @@ int main(int argc, char* argv[])
         return 1;
     }
 
+    sdk_heartbeat(client);
+    poll_message(client, &msg);
+    if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
+        fprintf(stderr, "Did not receive heartbeat\n");
+        return 1;
+    }
+
     int c;
     while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
         switch(c) {
@@ -70,11 +77,6 @@ int main(int argc, char* argv[])
                 		atof(argv[optind + 0]),
                 		atof(argv[optind + 1]),
                 		atof(argv[optind + 2]) );
-                poll_message(client, &msg);
-                if(msg.header.cmdid != CHASSIS_SPEED_MODE_CMDID || msg.resp.chsspeed.retcode) {
-                    fprintf(stderr, "Could not set speed\n");
-                    return 1;
-                }
 		break;
             case '?':
             default: