Initial Commit

src/robomaster.c

@@ -0,0 +1,58 @@
+#include <stdlib.h>
+#include <string.h>
+#include <arpa/inet.h>
+
+#include "robomaster.h"
+
+struct Session* session_new(void* buffer, size_t size) {
+    struct Session* session = malloc(sizeof(struct Session*));
+    if(!buffer)
+        session->buffer = malloc(sizeof(struct Header));
+    else
+        session->buffer = buffer;
+    session->size = 0;
+    session->seq = RM_SDK_FIRST_SEQ_ID;
+    session->max_size = size;
+    return session;
+}
+
+size_t session_size(Session session) {
+    return session->size;
+}
+
+// private
+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;
+    length += sizeof(struct Header) + sizeof(struct Footer);
+    session->size += length;
+
+    message->header.preamble = 0x55;
+    message->header.length = htons(length & 0xff03 | 4);
+    message->header.crc = crc8(message, 3);
+    message->header.seq_id = htons(session->seq++);
+    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));
+
+    return message;
+
+}
+
+// public
+const Message
+set_sdk_connection_req_new (
+		Session session,
+		const struct SetSdkConnectionReq* msg ) {
+
+    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;
+}