diff --git a/libuavcan/include/uavcan/internal/transport/transfer.hpp b/libuavcan/include/uavcan/internal/transport/transfer.hpp new file mode 100644 index 0000000000..e5609ed9e6 --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/transfer.hpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 + */ + +#pragma once + +#include +#include +#include + +namespace uavcan +{ + +enum TransferType +{ + SERVICE_RESPONSE = 0, + SERVICE_REQUEST = 1, + MESSAGE_BROADCAST = 2, + MESSAGE_UNICAST = 3 +}; + +struct Frame +{ + enum { MAX_TRANSFER_ID = 15 }; + + uint8_t payload[8]; + TransferType transfer_type; + uint_fast16_t data_type_id; + uint_fast8_t payload_len; + uint_fast8_t source_node_id; + uint_fast8_t frame_index; + uint_fast8_t transfer_id; + bool last_frame; + + Frame(const uint8_t* payload, uint_fast8_t payload_len, uint_fast16_t data_type_id, TransferType transfer_type, + uint_fast8_t source_node_id, uint_fast8_t frame_index, uint_fast8_t transfer_id, bool last_frame) + : transfer_type(transfer_type) + , data_type_id(data_type_id) + , payload_len(payload_len) + , source_node_id(source_node_id) + , frame_index(frame_index) + , transfer_id(transfer_id) + , last_frame(last_frame) + { + assert(payload && payload_len <= 8); + std::copy(payload, payload + payload_len, this->payload); + } + + static Frame parse(const CanFrame& can_frame); + + /** + * Difference computed from (this.transfer_id - rhs.transfer_id) with proper overflow handling. + */ + int subtractTransferID(const Frame& rhs) const { return subtractTransferID(rhs.transfer_id); } + int subtractTransferID(int rhs) const; + + bool operator!=(const Frame& rhs) const { return !operator==(rhs); } + bool operator==(const Frame& rhs) const + { + return + (transfer_type == rhs.transfer_type) && + (data_type_id == rhs.data_type_id) && + (source_node_id == rhs.source_node_id) && + (frame_index == rhs.frame_index) && + (transfer_id == rhs.transfer_id) && + (last_frame == rhs.last_frame) && + (payload_len == rhs.payload_len) && + std::equal(payload, payload + payload_len, rhs.payload); + } +}; + +struct RxFrame +{ + uint_fast64_t timestamp; + Frame frame; + uint_fast8_t iface_index; + + RxFrame(const Frame& frame, uint_fast64_t timestamp, uint_fast8_t iface_index) + : timestamp(timestamp) + , frame(frame) + , iface_index(iface_index) + { } + + static RxFrame parse(const CanRxFrame& can_frame) + { + return RxFrame(Frame::parse(can_frame.frame), can_frame.timestamp, can_frame.iface_index); + } +}; + +} diff --git a/libuavcan/src/transport/transfer.cpp b/libuavcan/src/transport/transfer.cpp new file mode 100644 index 0000000000..21dd7b263f --- /dev/null +++ b/libuavcan/src/transport/transfer.cpp @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2014 + */ + +#include + +namespace uavcan +{ + +//static Frame Frame::parse(const CanFrame& can_frame) +//{ +//} + +int Frame::subtractTransferID(int rhs) const +{ + static const int RANGE = MAX_TRANSFER_ID + 1; // 16 256 + static const int NEGATIVE = -RANGE / 2; // -8 -128 (two's complement) + static const int POSITIVE = (-NEGATIVE) - 1; // 7 127 + + const int d = int(this->transfer_id) - rhs; + if (d <= NEGATIVE) + return RANGE + d; + else if (d >= POSITIVE) + return d - RANGE; + return d; +} + +}