26 lines
736 B
C++
26 lines
736 B
C++
|
#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;
|
||
|
|
}
|