25 lines
829 B
C++
25 lines
829 B
C++
#include "protocol.hpp"
|
|
#include <stdint.h>
|
|
|
|
#include "pico/stdlib.h"
|
|
|
|
#include "crc16.hpp"
|
|
|
|
void assembleGobotRPCHeader(char * buffer, GobotRPCNumber number, GobotRPCTypes type, size_t len) {
|
|
buffer[0] = ((number & 0b1111) << 4) | ((type & 0b11) << 2);
|
|
buffer[1] = (len + 4) & 0xff;
|
|
}
|
|
|
|
GobotRPCHeaderInfo extractGobotRPCHeader(char * buffer) {
|
|
GobotRPCHeaderInfo info;
|
|
info.number = (GobotRPCNumber)((buffer[0] >> 4) & 0b1111);
|
|
info.type = (GobotRPCTypes)((buffer[0] >> 2) & 0b11);
|
|
info.len = buffer[1];
|
|
return info;
|
|
}
|
|
|
|
void assembleCRC(char * buffer, size_t data_len) {
|
|
unsigned short crc = crc16(buffer, data_len + GobotRPC_Package_DATA_OFFSET);
|
|
buffer[GobotRPC_Package_DATA_OFFSET + data_len + 1] = crc & 0xff;
|
|
buffer[GobotRPC_Package_DATA_OFFSET + data_len + 0] = (crc >> 8) & 0xff;
|
|
} |