build command parser

CommandParser.cpp

@@ -0,0 +1,25 @@
+#include "CommandParser.hpp"
+#include "UartHandler.hpp"
+#include "StateMachine.hpp"
+
+CommandParser &CommandParser::getInstance() noexcept
+{
+    static CommandParser instance;
+    return instance;
+}
+
+CommandParser::Command CommandParser::parsePacket(const uint8_t *packet) noexcept
+{
+    Command command;
+    command.instruction = (Command::TYPE)packet[0];
+    // Fetch args by big endian byte ordering
+    command.args[0] = ((uint16_t)packet[1] << 8) | packet[2];
+    command.args[1] = ((uint16_t)packet[3] << 8) | packet[4];
+
+    // Validate instruction
+    if ((uint8_t)command.instruction > 0x02) {
+        StateMachine::getInstance().raiseError("Received unknown instruction");
+    }
+
+    return command;
+}