First

src/command_message.c

@@ -0,0 +1,73 @@
+#include "command_message.h"
+
+#include <string.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
+
+void command_message_init(struct command_message_t *msg) {
+	memset(msg, 0, sizeof(struct command_message_t));
+	msg->prefix = COMMAND_PREFIX;
+	msg->id = COMMAND_ID;
+}
+
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
+	// Ensure length doesn't exceed available space
+	if (length > sizeof(msg->data) - 1) {
+		return;
+	}
+
+	command_message_init(msg);
+	msg->length = length;
+	msg->command = command;
+
+	// Copy the data
+	if (data != NULL) {
+		for (int i = 0; i < msg->length; i++) {
+			msg->data[i] = data[i];
+		}
+	}
+	msg->crc = command_calculate_crc(msg);
+}
+
+uint8_t command_calculate_crc(struct command_message_t *msg) {
+	uint32_t sum = 0;
+	uint8_t crc = 0;
+
+	uint8_t *byte_ptr = (uint8_t *)msg;
+
+	for (int i = 0; i < (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length) - 1; i++) {
+		sum += byte_ptr[i];
+	}
+
+	crc = 0x100 - (sum & 0xff);
+
+	return crc;
+}
+
+void command_create_ack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_ACK, NULL);
+}
+
+void command_create_nack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_NACK, NULL);
+}
+
+void command_log(struct command_message_t *msg) {
+	if (msg->length > sizeof(msg->data) - 1) {
+		LOG_ERR("Message length too long: %d", msg->length);
+		return;
+	}
+
+	LOG_INF("Prefix: %d\n\r", msg->prefix);
+	LOG_INF("Length: %d\n\r", msg->length);
+	LOG_INF("COMMAND_ID: %d\n\r", msg->id);
+	LOG_INF("Command: %d\n\r", msg->command);
+	LOG_INF("Data:\n\r");
+
+	for (int i = 0; i < msg->length; i++) {
+		LOG_INF("%d", msg->data[i]);
+	}
+
+	LOG_INF("CRC: %d\n\r", msg->crc);
+}