Martino Ferrari
121 lines
3.9 KiB
C++
121 lines
3.9 KiB
C++
#pragma once
|
|
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <cstring>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <stdexcept>
|
|
|
|
// ─── Protocol constants (matching UDPStreamer wire format) ──────────
|
|
|
|
constexpr uint32_t UDPS_MAGIC = 0x53504455u; // 'UDPS' LE
|
|
constexpr uint8_t UDPS_TYPE_DATA = 0u;
|
|
constexpr uint8_t UDPS_TYPE_CONFIG = 1u;
|
|
constexpr uint8_t UDPS_TYPE_ACK = 2u;
|
|
constexpr uint8_t UDPS_TYPE_CONNECT = 3u;
|
|
constexpr uint8_t UDPS_TYPE_DISCONNECT = 4u;
|
|
constexpr size_t UDPS_HEADER_SIZE = 17;
|
|
constexpr size_t UDPS_SIGDESC_SIZE = 136;
|
|
|
|
// Type codes (matching Go protocol.go)
|
|
constexpr uint8_t TC_UINT8 = 0u;
|
|
constexpr uint8_t TC_INT8 = 1u;
|
|
constexpr uint8_t TC_UINT16 = 2u;
|
|
constexpr uint8_t TC_INT16 = 3u;
|
|
constexpr uint8_t TC_UINT32 = 4u;
|
|
constexpr uint8_t TC_INT32 = 5u;
|
|
constexpr uint8_t TC_UINT64 = 6u;
|
|
constexpr uint8_t TC_INT64 = 7u;
|
|
constexpr uint8_t TC_FLOAT32 = 8u;
|
|
constexpr uint8_t TC_FLOAT64 = 9u;
|
|
|
|
// Quantisation types
|
|
constexpr uint8_t QUANT_NONE = 0u;
|
|
constexpr uint8_t QUANT_UINT8 = 1u;
|
|
constexpr uint8_t QUANT_INT8 = 2u;
|
|
constexpr uint8_t QUANT_UINT16 = 3u;
|
|
constexpr uint8_t QUANT_INT16 = 4u;
|
|
|
|
// Time modes
|
|
constexpr uint8_t TM_PACKET = 0u;
|
|
constexpr uint8_t TM_FULL_ARRAY = 1u;
|
|
constexpr uint8_t TM_FIRST_SAMPLE = 2u;
|
|
constexpr uint8_t TM_LAST_SAMPLE = 3u;
|
|
|
|
constexpr uint32_t NO_TIME_SIGNAL = 0xFFFFFFFFu;
|
|
|
|
#pragma pack(push, 1)
|
|
struct UDPSPacketHeader {
|
|
uint32_t magic;
|
|
uint8_t type;
|
|
uint32_t counter;
|
|
uint16_t fragment_idx;
|
|
uint16_t total_fragments;
|
|
uint32_t payload_bytes;
|
|
};
|
|
|
|
struct UDPSSignalInfo {
|
|
char name[64];
|
|
uint8_t type_code;
|
|
uint8_t quant_type;
|
|
uint8_t num_dimensions;
|
|
uint32_t num_rows;
|
|
uint32_t num_cols;
|
|
double range_min;
|
|
double range_max;
|
|
uint8_t time_mode;
|
|
double sampling_rate;
|
|
uint32_t time_signal_idx;
|
|
char unit[32];
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
static_assert(sizeof(UDPSPacketHeader) == 17, "Header must be 17 bytes");
|
|
static_assert(sizeof(UDPSSignalInfo) == 136, "Signal desc must be 136 bytes");
|
|
|
|
// ─── Parsed signal metadata ─────────────────────────────────────────
|
|
|
|
struct SignalDef {
|
|
std::string name;
|
|
uint8_t type_code = 0;
|
|
uint8_t quant_type = 0;
|
|
uint32_t num_elements = 0; // rows * cols
|
|
uint32_t num_rows = 1;
|
|
uint32_t num_cols = 1;
|
|
double range_min = 0.0;
|
|
double range_max = 0.0;
|
|
uint8_t time_mode = TM_PACKET;
|
|
double sampling_rate = 0.0;
|
|
uint32_t time_signal_idx = NO_TIME_SIGNAL;
|
|
std::string unit;
|
|
|
|
int element_byte_size() const;
|
|
};
|
|
|
|
// ─── One DATA packet worth of samples ───────────────────────────────
|
|
|
|
struct DataPacket {
|
|
uint64_t hrt_timestamp = 0;
|
|
// Per-signal float values (always float64 after dequantisation)
|
|
std::vector<std::vector<double>> values;
|
|
};
|
|
|
|
// ─── Parser functions ───────────────────────────────────────────────
|
|
|
|
/** Validate a UDP datagram header. Returns false if magic or size is wrong. */
|
|
bool parse_header(const uint8_t* buf, size_t len, UDPSPacketHeader& hdr);
|
|
|
|
/** Parse a reassembled CONFIG payload into a signal list. */
|
|
std::vector<SignalDef> parse_config(const uint8_t* payload, size_t len);
|
|
|
|
/** Parse a reassembled DATA payload into float64 values per signal. */
|
|
DataPacket parse_data(const uint8_t* payload, size_t len,
|
|
const std::vector<SignalDef>& sig_defs);
|
|
|
|
/** Build a 17-byte CONNECT packet. */
|
|
std::vector<uint8_t> build_connect();
|
|
|
|
/** Build a 17-byte DISCONNECT packet. */
|
|
std::vector<uint8_t> build_disconnect();
|