Identical output to original Robomaster SDK

src/robomaster.c

@@ -13,6 +13,8 @@ struct Session* session_new(void* buffer, size_t size) {
     session->size = 0;
     session->seq = RM_SDK_FIRST_SEQ_ID;
     session->max_size = size;
+    // TODO: Make this configurable
+    session->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX);
     return session;
 }
 
@@ -25,21 +27,25 @@ struct Message*
 message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body) {
 
     struct Message* message = session->buffer + session->size;
+    memcpy((void*)message->body, body, length);
     length += sizeof(struct Header) + sizeof(struct Footer);
     session->size += length;
+    int16_t seq = ++session->seq;
 
     message->header.preamble = 0x55;
-    message->header.length = htons(length & 0xff03 | 4);
+    message->header.length_l = length & 0xFF;
+    message->header.length_h = (length >> 8) & 0x3 | 4;
     message->header.crc = crc8(message, 3);
-    message->header.seq_id = htons(session->seq++);
+    message->header.seq_id = seq;
+    message->header.sender = session->hostbyte;
+    message->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
     message->header.ack_needed = true;
     message->header.cmdset = cmdset;
     message->header.cmdid = cmdid;
 
-    memcpy((void*)message->body, body, length);
-
     struct Footer* footer = (void*)message + length - sizeof(struct Footer);
-    footer->crc = htons(crc16(message, length - 2));
+    uint16_t crc = crc16(message, length - sizeof(struct Footer));
+    footer->crc = crc;
 
     return message;
 
@@ -49,10 +55,9 @@ message_new(struct Session* session, uint8_t cmdset, uint8_t cmdid, size_t lengt
 const Message
 set_sdk_connection_req_new (
 		Session session,
-		const struct SetSdkConnectionReq* msg ) {
+		struct SetSdkConnectionReq* msg ) {
 
+    msg->host = session->hostbyte;
     struct Message* message = message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), msg);
-    struct SetSdkConnectionReq* body = (struct SetSdkConnectionReq*)message->body;
-    body->port = htons(body->port);
     return message;
 }