diff --git a/axiomatic_adapter/CMakeLists.txt b/axiomatic_adapter/CMakeLists.txt index 7fab564..e453477 100644 --- a/axiomatic_adapter/CMakeLists.txt +++ b/axiomatic_adapter/CMakeLists.txt @@ -45,7 +45,13 @@ find_package(CLI11 REQUIRED) find_package(socketcan_adapter REQUIRED) # Axiomatic Adapter Library -add_library(axiomatic_adapter SHARED src/axiomatic_adapter.cpp) +add_library(axiomatic_adapter SHARED + src/axiomatic_adapter.cpp + src/axiomatic_model.cpp + src/axiomatic_protocol.cpp + src/models/ax140900.cpp + src/models/ax142100a.cpp +) target_compile_features(axiomatic_adapter PUBLIC c_std_99 cxx_std_17) target_include_directories(axiomatic_adapter PUBLIC @@ -98,6 +104,14 @@ if(BUILD_TESTING) endif() include(Catch OPTIONAL) + add_executable(test_axiomatic_model test/axiomatic_model_test.cpp) + target_link_libraries(test_axiomatic_model PRIVATE axiomatic_adapter Catch2::Catch2WithMain) + if(COMMAND catch_discover_tests) + catch_discover_tests(test_axiomatic_model) + else() + add_test(NAME test_axiomatic_model COMMAND test_axiomatic_model) + endif() + option(HARDWARE_CONNECTED "Enable tests that require a connected Axiomatic device and CAN interface" OFF) if(HARDWARE_CONNECTED) diff --git a/axiomatic_adapter/README.md b/axiomatic_adapter/README.md index 56a3a8f..01aabcc 100644 --- a/axiomatic_adapter/README.md +++ b/axiomatic_adapter/README.md @@ -1,22 +1,43 @@ # Axiomatic Adapter -Library and Adapter for the Axiomatic CAN-ETH converter. +Library and Adapter for Axiomatic CAN-Ethernet converters. -For more information on the decoding/endcoding and product, see the links below: +For more information on the decoding/encoding, see: https://www.notion.so/polymathrobotics/Axiomatic-CAN-to-Ethernet-Converter-08e078d8914f40d6b7cd99ebf39fe1b0 -https://products.axiomatic.com/viewitems/connectivity/ethernet-can-converters +## Supported Models + +| `--model` | Device | Protocol reference | +| --- | --- | --- | +| `ax140900` (default) | [AX140900 CAN/Ethernet Converter](https://www.axiomatic.com/product/canethernet-converter-ax140900/) | [Ethernet to CAN Converter Communication Protocol](https://www.axiomatic.com/wp-content/uploads/Ethernet-to-CAN-Converter-Communication-Protocol.pdf) | +| `ax142100a` | [AX142100A Protocol Converter, Ethernet/RS-422/2x RS-232/CAN](https://www.axiomatic.com/product/protocol-converter-ethernet-rs-422-2-rs-232-can-sae-j1939-ax142100a/) | [UMAX142100A](https://www.axiomatic.com/wp-content/uploads/UMAX142100A.pdf), section 4.2 | + +Every model shares the 11-byte `AXIO` message header in `axiomatic_protocol.hpp`. +Each model is a subclass of `AxiomaticModel` (`axiomatic_model.hpp`) in `include/axiomatic_adapter/models/` and `src/models/`. +A model takes bytes and `CanFrame`s only, so it can be used without a socket. + +### Adding a model + +1. Add `include/axiomatic_adapter/models/.hpp` and `src/models/.cpp` with a `::` class deriving `AxiomaticModel`. + Override `protocolId`, `encode` (build on `protocol::encodeMessage`), and `decodeMessage` (one message body; return `false` for Message IDs that carry no CAN frames). + The base class `decode` splits the buffer into messages and calls `decodeMessage` for each. +2. Add the source to the `axiomatic_adapter` library in `CMakeLists.txt`. +3. Add its entry to `modelFactories` in `src/axiomatic_model.cpp`. +4. Add a decode test of the manual's example bytes in `test/axiomatic_model_test.cpp`; the round trip test covers every name in `modelNames`. ## Usage ### Socketcan-Axiomatic Bridge ```bash -ros2 run axiomatic_adapter axiomatic_socketcan_bridge [CAN_INTERFACE_NAME] [IP_ADDRESS] [PORT] [OPTIONAL]--retry-connection[-r] [OPTIONAL]--max-retry-attempts [OPTIONAL]--verbose[-v] [OPTIONAL]--no-tcp-nodelay +ros2 run axiomatic_adapter axiomatic_socketcan_bridge [CAN_INTERFACE_NAME] [IP_ADDRESS] [PORT] [OPTIONAL]--model[-m] [OPTIONAL]--retry-connection[-r] [OPTIONAL]--max-retry-attempts [OPTIONAL]--verbose[-v] [OPTIONAL]--no-tcp-nodelay # Examples # generic example to bridge vcan0 with axiomatic using 192.168.50.34:4000 ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 +# bridge vcan0 with an AX142100A +ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 --model ax142100a + # this will continue retrying to connect forever and not exit on first failure. It will also print more detailed logs ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 -r -v @@ -44,7 +65,8 @@ polymath::can::AxiomaticAdapter adapter( [](std::unique_ptr /*frame*/) { /* No-op */ }, [](polymath::can::AxiomaticAdapter::socket_error_string_t /*error*/) { /*do nothing*/ }, receive_timeout_ms, - /*tcp_nodelay=*/true // optional, defaults to true; see below + /*tcp_nodelay=*/true, // optional, defaults to true; see below + std::make_unique() // optional, defaults to ax140900::Ax140900 ); // open the socket @@ -81,3 +103,5 @@ If you have any doubt, leave it at the default. 2. Axiomatic-specific status messages are ignored and deliberately skipped. In future revisions this should be handled and reported as necessary. 3. CAN FD is not supported 4. Only TCP mode is supported; no UDP support (heartbeats are required for UDP support) +5. AX142100A raw data (serial) payloads are skipped; only CAN frames are delivered. +6. AX142100A 11-bit IDs are assumed to be 2 bytes on the wire; the manual only shows a 29-bit example. diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp index 9c34752..5b541b0 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp @@ -24,11 +24,11 @@ #include #include +#include "axiomatic_adapter/axiomatic_model.hpp" +#include "axiomatic_adapter/models/ax140900.hpp" #include "socketcan_adapter/can_frame.hpp" -namespace polymath -{ -namespace can +namespace polymath::can { /// @brief State of TCP socket, error, open or closed @@ -66,6 +66,7 @@ class AxiomaticAdapter : public std::enable_shared_from_this /// on both sides, more switch/NIC PPS load, and worse bytes-on-wire /// efficiency for bulk transfers). Better for real-time, but can, in constrained /// resource environments, cause issues + /// @param model converter model, e.g. std::make_unique(); must not be null AxiomaticAdapter( const std::string & ip_address, const std::string & port, @@ -74,7 +75,8 @@ class AxiomaticAdapter : public std::enable_shared_from_this const std::function && error_callback_function = [](socket_error_string_t /*error*/) { /*do nothing*/ }, const std::chrono::milliseconds & receive_timeout_ms = AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, - bool tcp_nodelay = true); + bool tcp_nodelay = true, + std::unique_ptr model = std::make_unique()); /// @brief Destructor for AxiomaticAdapter virtual ~AxiomaticAdapter(); @@ -129,7 +131,6 @@ class AxiomaticAdapter : public std::enable_shared_from_this class AxiomaticAdapterImpl; std::unique_ptr pimpl_; }; -} // namespace can -} // namespace polymath +} // namespace polymath::can #endif // AXIOMATIC_ADAPTER__AXIOMATIC_ADAPTER_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp new file mode 100644 index 0000000..f0bc466 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AXIOMATIC_ADAPTER__AXIOMATIC_MODEL_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_MODEL_HPP_ + +#include +#include +#include +#include +#include + +#include "axiomatic_adapter/axiomatic_protocol.hpp" +#include "socketcan_adapter/can_frame.hpp" + +namespace polymath::can +{ + +/// @class polymath::can::AxiomaticModel +/// @brief One Axiomatic converter model and its wire format. +/// Subclasses override protocolId, encode, and decodeMessage. +/// Methods are const and keep no state between calls; one instance may be shared across threads. +class AxiomaticModel +{ +public: + virtual ~AxiomaticModel() = default; + + /// @brief Encode one CAN frame as one complete protocol message + virtual std::vector encode(const polymath::socketcan::CanFrame & frame) const = 0; + + /// @brief Decode every CAN frame in a buffer of back-to-back protocol messages. + /// Messages with a Message ID that carries no CAN frames are skipped. + /// @param data buffer start + /// @param size buffer length in bytes + protocol::DecodeResult decode(const uint8_t * data, size_t size) const; + +protected: + /// @brief Protocol ID every message of this model carries + virtual uint16_t protocolId() const = 0; + + /// @brief Append the CAN frames in one message to result + /// @return false when the Message ID carries no CAN frames + virtual bool decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const = 0; +}; + +/// @brief Construct the model with a lower-case name, e.g. "ax142100a" +/// @throws std::invalid_argument for a name not in modelNames() +std::unique_ptr makeModel(const std::string & model); + +/// @brief Every model name makeModel accepts +std::vector modelNames(); + +} // namespace polymath::can + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_MODEL_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp new file mode 100644 index 0000000..a3034e5 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ + +#include +#include +#include +#include +#include + +#include "socketcan_adapter/can_frame.hpp" + +namespace polymath::can::protocol +{ + +/// @brief Message Header framing shared by every Axiomatic Ethernet converter. +/// Wire layout, multi-byte fields LSB first: +/// bytes 0-3 : Axiomatic Tag "AXIO" +/// bytes 4-5 : Protocol ID (model specific) +/// bytes 6-7 : Message ID +/// byte 8 : Message Version +/// bytes 9-10 : Message Data Length (body bytes that follow the header) +static constexpr std::array AXIOMATIC_TAG = {'A', 'X', 'I', 'O'}; +static constexpr size_t HEADER_BYTES = 11; +static constexpr size_t PROTOCOL_ID_OFFSET = 4; +static constexpr size_t MESSAGE_ID_OFFSET = 6; +static constexpr size_t MESSAGE_VERSION_OFFSET = 8; +static constexpr size_t MESSAGE_DATA_LENGTH_OFFSET = 9; + +/// @brief One protocol message located inside a receive buffer +struct MessageView +{ + uint16_t message_id; + uint8_t message_version; + /// @brief points into the buffer passed to parseMessages; valid only while that buffer is + const uint8_t * body; + size_t body_size; +}; + +/// @brief Protocol messages found in a buffer, plus a description of every byte range skipped +struct ParsedMessages +{ + std::vector messages; + std::vector diagnostics; +}; + +/// @brief CAN frames decoded from a buffer, plus a description of every byte range skipped or dropped +struct DecodeResult +{ + std::vector frames; + std::vector diagnostics; +}; + +/// @brief Read an unsigned little-endian integer +/// @param data first byte, LSB +/// @param num_bytes field width, at most 4 +uint32_t readLittleEndian(const uint8_t * data, size_t num_bytes); + +/// @brief Append the low num_bytes of value, LSB first +void appendLittleEndian(std::vector & out, uint32_t value, size_t num_bytes); + +/// @brief Frame a message body with the Axiomatic Message Header +/// @param protocol_id model specific Protocol ID +/// @param message_id Message ID for the body type +/// @param message_version Message Version +/// @param body Message Data, at most 65535 bytes +/// @return header followed by body +std::vector encodeMessage( + uint16_t protocol_id, uint16_t message_id, uint8_t message_version, const std::vector & body); + +/// @brief Split a buffer into back-to-back protocol messages carrying protocol_id. +/// Scanning stops at the first header whose tag or Protocol ID does not match. +/// A message whose declared length runs past the buffer is returned truncated to the bytes present. +/// @param protocol_id Protocol ID every message must carry +/// @param data buffer start +/// @param size buffer length in bytes +ParsedMessages parseMessages(uint16_t protocol_id, const uint8_t * data, size_t size); + +} // namespace polymath::can::protocol + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp index e2841cd..271d3b7 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp @@ -15,15 +15,15 @@ #ifndef AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ #define AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ +#include #include #include "axiomatic_adapter/axiomatic_adapter.hpp" +#include "axiomatic_adapter/models/ax140900.hpp" #include "socketcan_adapter/can_frame.hpp" #include "socketcan_adapter/socketcan_adapter.hpp" -namespace polymath -{ -namespace can +namespace polymath::can { /// @class polymath::can::AxiomaticSocketcanBridge @@ -39,12 +39,14 @@ class AxiomaticSocketcanBridge /// @param tcp_nodelay when true (default), sets TCP_NODELAY on the underlying /// AxiomaticAdapter socket. See AxiomaticAdapter for the /// full tradeoff discussion. + /// @param model converter model, e.g. std::make_unique(); must not be null AxiomaticSocketcanBridge( const std::string & can_interface_name, const std::string & ip, const std::string & port, bool verbose = false, - bool tcp_nodelay = true); + bool tcp_nodelay = true, + std::unique_ptr model = std::make_unique()); /// @brief Destruct axiomatic socketcan bridge ~AxiomaticSocketcanBridge(); @@ -75,7 +77,6 @@ class AxiomaticSocketcanBridge void ethcanReceiveCallback(std::unique_ptr frame); }; -} // namespace can -} // namespace polymath +} // namespace polymath::can #endif // AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/models/ax140900.hpp b/axiomatic_adapter/include/axiomatic_adapter/models/ax140900.hpp new file mode 100644 index 0000000..25fc5fb --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/models/ax140900.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AXIOMATIC_ADAPTER__MODELS__AX140900_HPP_ +#define AXIOMATIC_ADAPTER__MODELS__AX140900_HPP_ + +#include +#include + +#include "axiomatic_adapter/axiomatic_model.hpp" +#include "axiomatic_adapter/axiomatic_protocol.hpp" +#include "socketcan_adapter/can_frame.hpp" + +/// @brief AX140900 CAN/Ethernet Converter, "Ethernet to CAN Converter Communication Protocol" v6. +/// https://www.axiomatic.com/product/canethernet-converter-ax140900/ +/// https://www.axiomatic.com/wp-content/uploads/Ethernet-to-CAN-Converter-Communication-Protocol.pdf +namespace polymath::can::ax140900 +{ + +/// @brief Protocol ID, 0xBA 0x36 on the wire +static constexpr uint16_t PROTOCOL_ID = 0x36BA; + +/// @brief Message IDs (spec v6, table 3) +enum class MessageId : uint16_t +{ + Undefined = 0, + CanStream = 1, + StatusRequest = 2, + StatusResponse = 3, + Heartbeat = 4, + CanFdStream = 5, +}; + +/// @class polymath::can::ax140900::Ax140900 +/// @brief Encodes a single-frame CAN Stream message with a fixed 2-byte time stamp. +/// Decodes CAN Stream messages; notification frames are skipped. +class Ax140900 : public AxiomaticModel +{ +public: + std::vector encode(const polymath::socketcan::CanFrame & frame) const override; + +protected: + uint16_t protocolId() const override; + bool decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const override; +}; + +} // namespace polymath::can::ax140900 + +#endif // AXIOMATIC_ADAPTER__MODELS__AX140900_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp b/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp new file mode 100644 index 0000000..8392895 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AXIOMATIC_ADAPTER__MODELS__AX142100A_HPP_ +#define AXIOMATIC_ADAPTER__MODELS__AX142100A_HPP_ + +#include +#include + +#include "axiomatic_adapter/axiomatic_model.hpp" +#include "axiomatic_adapter/axiomatic_protocol.hpp" +#include "socketcan_adapter/can_frame.hpp" + +/// @brief AX142100A RS232-RS232-RS422-ENET-CAN converter, UMAX142100A section 4.2. +/// https://www.axiomatic.com/product/protocol-converter-ethernet-rs-422-2-rs-232-can-sae-j1939-ax142100a/ +/// https://www.axiomatic.com/wp-content/uploads/UMAX142100A.pdf +namespace polymath::can::ax142100a +{ + +/// @brief Protocol ID 20008, 0x28 0x4E on the wire +static constexpr uint16_t PROTOCOL_ID = 0x4E28; + +/// @brief Message IDs (UMAX142100A section 4.2) +enum class MessageId : uint16_t +{ + Undefined = 0, + ForwardedData = 1, +}; + +/// @class polymath::can::ax142100a::Ax142100a +/// @brief Encodes a Forwarded Data message. +/// Decodes Forwarded Data messages; raw data payloads are skipped. +class Ax142100a : public AxiomaticModel +{ +public: + std::vector encode(const polymath::socketcan::CanFrame & frame) const override; + +protected: + uint16_t protocolId() const override; + bool decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const override; +}; + +} // namespace polymath::can::ax142100a + +#endif // AXIOMATIC_ADAPTER__MODELS__AX142100A_HPP_ diff --git a/axiomatic_adapter/src/axiomatic_adapter.cpp b/axiomatic_adapter/src/axiomatic_adapter.cpp index ceaa88a..1a5e381 100644 --- a/axiomatic_adapter/src/axiomatic_adapter.cpp +++ b/axiomatic_adapter/src/axiomatic_adapter.cpp @@ -17,12 +17,13 @@ #include #include -#include #include #include #include #include +#include #include +#include #include #include #include @@ -32,9 +33,7 @@ #include #include -namespace polymath -{ -namespace can +namespace polymath::can { class AxiomaticAdapter::AxiomaticAdapterImpl @@ -46,7 +45,8 @@ class AxiomaticAdapter::AxiomaticAdapterImpl const std::function frame)> && receive_callback_function, const std::function && error_callback_function, const std::chrono::milliseconds & receive_timeout_ms, - bool tcp_nodelay) + bool tcp_nodelay, + std::unique_ptr model) : tcp_io_context_() , tcp_socket_(tcp_io_context_) , ip_address_(ip_address) @@ -56,7 +56,12 @@ class AxiomaticAdapter::AxiomaticAdapterImpl , receive_timeout_ms_(receive_timeout_ms) , rx_buffer_(RECEIVE_BUFFER_SIZE, 0) , tcp_nodelay_(tcp_nodelay) - {} + , model_(std::move(model)) + { + if (!model_) { + throw std::invalid_argument("AxiomaticAdapter requires a model"); + } + } ~AxiomaticAdapterImpl() { @@ -200,8 +205,8 @@ class AxiomaticAdapter::AxiomaticAdapterImpl std::optional receive(polymath::socketcan::CanFrame & can_frame) { - // A previous TCP read may have decoded several CAN frames out of a single - // packed CAN Stream message — deliver those one at a time before doing + // A previous TCP read may have decoded several CAN frames out of + // packed protocol messages — deliver those one at a time before doing // another network read, so packed frames don't get silently dropped if (!pending_frames_.empty()) { can_frame = pending_frames_.front(); @@ -250,108 +255,24 @@ class AxiomaticAdapter::AxiomaticAdapterImpl "Receive operation failed: " + error_code.message()); } - // --- Process the received data --- - // walk the buffer starting at position 0, dispatching every Axiomatic protocol message we find by its Message ID - // NOTE: given the axiomatic documentation claims a deliberate 256-byte design + the large buffer size used in // rx_buffer_, a mid-tcp-message split is never supposed to occur. In testing this drop has never happened. // it is possible that with non-standard very low MTU's or in future revisions, this assumption no longer holds - if (bytes_received < PROTOCOL_HEADER_BYTES) { - std::cerr << "[Axiomatic parser] DROP: received " << bytes_received - << " bytes, too short to contain a complete protocol header" << std::endl; - return std::make_optional("Data too short for header."); - } - - size_t scan_pos = 0; - while (scan_pos + PROTOCOL_HEADER_BYTES <= bytes_received) { - auto protocol_header_match = std::equal( - AXIOMATIC_CAN_MESSAGE_HEADER.begin(), - AXIOMATIC_CAN_MESSAGE_HEADER.begin() + PROTOCOL_SYNC_PREFIX_BYTES, - rx_buffer_.begin() + scan_pos); - if (!protocol_header_match) { - std::cerr << "[Axiomatic parser] DROP: sync prefix mismatch at offset " << scan_pos << " of " << bytes_received - << "-byte TCP read; first " << PROTOCOL_SYNC_PREFIX_BYTES << " bytes there: " << std::hex; - for (size_t i = 0; i < PROTOCOL_SYNC_PREFIX_BYTES && scan_pos + i < bytes_received; ++i) { - std::cerr << ' ' << static_cast(rx_buffer_[scan_pos + i]); - } - std::cerr << std::dec << " (stopping scan; remaining " << (bytes_received - scan_pos) - << " bytes ignored — possible truncated message or partial TCP read)" << std::endl; - break; - } - const auto msg_id = static_cast( - static_cast(rx_buffer_[scan_pos + MESSAGE_ID_HEADER_OFFSET]) | - (static_cast(rx_buffer_[scan_pos + MESSAGE_ID_HEADER_OFFSET + 1]) << BITS_PER_BYTE)); - const size_t decl_len = - static_cast(rx_buffer_[scan_pos + MESSAGE_DATA_LENGTH_HEADER_OFFSET]) | - (static_cast(rx_buffer_[scan_pos + MESSAGE_DATA_LENGTH_HEADER_OFFSET + 1]) << BITS_PER_BYTE); - const size_t body_end = std::min(scan_pos + PROTOCOL_HEADER_BYTES + decl_len, bytes_received); - if (msg_id == MessageId::CanStream) { - decodePackedCanFramesInto(rx_buffer_, scan_pos + PROTOCOL_HEADER_BYTES, body_end); - } else { - std::cerr << "[Axiomatic parser] SKIP: non-CAN-Stream message (Message ID " << static_cast(msg_id) - << ", " << decl_len << "-byte body) at offset " << scan_pos - << " — heartbeat/status/FD/unknown; not delivered to caller" << std::endl; - } - scan_pos += PROTOCOL_HEADER_BYTES + decl_len; + protocol::DecodeResult decoded = model_->decode(rx_buffer_.data(), bytes_received); + for (const auto & diagnostic : decoded.diagnostics) { + std::cerr << "[Axiomatic parser] " << diagnostic << std::endl; } - - if (pending_frames_.empty()) { - if (scan_pos == 0) { - return std::make_optional("Not a valid Axiomatic message."); - } - return std::make_optional("No CAN frames in received protocol traffic."); + if (decoded.frames.empty()) { + return std::make_optional( + decoded.diagnostics.empty() ? "No CAN frames in received protocol traffic." : decoded.diagnostics.back()); } + pending_frames_.insert(pending_frames_.end(), decoded.frames.begin(), decoded.frames.end()); can_frame = pending_frames_.front(); pending_frames_.pop_front(); return std::nullopt; } - // walks the body of a single CAN Stream message and pushes every CAN frame onto pending_frames_ - void decodePackedCanFramesInto(const std::vector & data_buffer, size_t body_start, size_t body_end) - { - size_t walker = body_start; - while (walker + CONTROL_BYTE_BYTES <= body_end) { - const uint8_t control_byte = data_buffer[walker]; - if ((control_byte & CONTROL_BYTE_NOTIFICATION_FRAME_FLAG) != 0) { - std::cerr << "[Axiomatic parser] SKIP: notification frame (CB=0x" << std::hex << static_cast(control_byte) - << std::dec << ") at offset " << walker << " — not delivered to caller" << std::endl; - walker += NOTIFICATION_FRAME_TOTAL_BYTES; - continue; - } - const size_t timestamp_size = TIMESTAMP_LENGTH_BYTES_TABLE - [(control_byte & CONTROL_BYTE_TIMESTAMP_LENGTH_MASK) >> CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT]; - const bool extended_id = (control_byte & CONTROL_BYTE_EXTENDED_ID_FLAG) != 0; - const size_t id_size = extended_id ? EXTENDED_CAN_ID_BYTES : STANDARD_CAN_ID_BYTES; - const size_t can_data_length = control_byte & CONTROL_BYTE_CAN_DATA_LENGTH_MASK; - const size_t frame_bytes = CONTROL_BYTE_BYTES + timestamp_size + id_size + can_data_length; - if (walker + frame_bytes > body_end) { - // truncated final frame — abandon rather than misdecode. - std::cerr << "[Axiomatic parser] DROP: truncated CAN frame at offset " << walker << " (CB=0x" << std::hex - << static_cast(control_byte) << std::dec << " declares " << frame_bytes << " bytes but only " - << (body_end - walker) << " bytes remain in message body) — frame and remainder dropped" << std::endl; - break; - } - const size_t id_offset = walker + CONTROL_BYTE_BYTES + timestamp_size; - uint32_t can_id = 0; - for (size_t i = 0; i < id_size; ++i) { - can_id |= static_cast(data_buffer[id_offset + i]) << (BITS_PER_BYTE * i); - } - std::array data_bytes = {0}; - std::copy_n(data_buffer.begin() + id_offset + id_size, can_data_length, data_bytes.begin()); - - polymath::socketcan::CanFrame extra; - extra.set_can_id(can_id); - extra.set_len(static_cast(can_data_length)); - extra.set_data(data_bytes); - if (extended_id) { - extra.set_id_as_extended(); - } - pending_frames_.push_back(extra); - walker += frame_bytes; - } - } - std::optional receive() { polymath::socketcan::CanFrame can_frame = polymath::socketcan::CanFrame(); @@ -361,55 +282,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl std::optional send(const polymath::socketcan::CanFrame & frame) { - auto frame_data = frame.get_data(); - auto frame_data_length = frame.get_len(); - - // the CAN Frame body emitted on the wire is laid out as: - // 1 byte Control Byte + 2 bytes Time Stamp + N bytes CAN ID + 8 bytes data - static constexpr size_t SEND_TIME_STAMP_BYTES = 2; - static constexpr size_t SEND_CONTROL_AND_TIMESTAMP_BYTES = CONTROL_BYTE_BYTES + SEND_TIME_STAMP_BYTES; - - // hard coded outbound time-stamp bytes (0x46C0) - static constexpr uint8_t SEND_TIME_STAMP_BYTE_0 = 192; - static constexpr uint8_t SEND_TIME_STAMP_BYTE_1 = 70; - - // reserved high bytes of the header that sit between Protocol ID and Message Data Length (spec calls this "Message ID high byte" + "Message - // version", both 0 for our outbound CAN Stream messages). - static constexpr uint8_t SEND_RESERVED_BYTE_0 = 0x00; - static constexpr uint8_t SEND_RESERVED_BYTE_1 = 0x00; - - size_t frame_id_byte_length; - bool is_extended = false; - if (frame.get_id_type() == polymath::socketcan::IdType::EXTENDED) { - frame_id_byte_length = EXTENDED_CAN_ID_BYTES; - is_extended = true; - } else { - frame_id_byte_length = STANDARD_CAN_ID_BYTES; - } - - size_t message_length = frame_data_length + SEND_CONTROL_AND_TIMESTAMP_BYTES + frame_id_byte_length; - unsigned char control_byte = CONTROL_BYTE_TIMESTAMP_2_BYTE_VALUE; - control_byte |= (is_extended ? CONTROL_BYTE_EXTENDED_ID_FLAG : 0); - control_byte |= (frame_data_length & CONTROL_BYTE_CAN_DATA_LENGTH_MASK); - - // initialize the full message with the header, control bytes, timestamp bytes - std::vector full_message; - full_message.insert(full_message.end(), AXIOMATIC_CAN_MESSAGE_HEADER.begin(), AXIOMATIC_CAN_MESSAGE_HEADER.end()); - full_message.push_back(SEND_RESERVED_BYTE_0); - full_message.push_back(SEND_RESERVED_BYTE_1); - full_message.push_back(static_cast(message_length & 0xFF)); - full_message.push_back(static_cast((message_length >> BITS_PER_BYTE) & 0xFF)); - full_message.push_back(control_byte); - full_message.push_back(SEND_TIME_STAMP_BYTE_0); - full_message.push_back(SEND_TIME_STAMP_BYTE_1); - - // insert the can frame id - auto can_id = frame.get_id(); - for (size_t i = 0; i < frame_id_byte_length; ++i) { - full_message.push_back(static_cast((can_id >> (i * BITS_PER_BYTE)) & 0xFF)); - } - // insert the can frame data - full_message.insert(full_message.end(), frame_data.begin(), frame_data.end()); + const std::vector full_message = model_->encode(frame); try { boost::asio::write(tcp_socket_, boost::asio::buffer(full_message.data(), full_message.size())); @@ -430,81 +303,11 @@ class AxiomaticAdapter::AxiomaticAdapterImpl } private: - static constexpr std::array AXIOMATIC_CAN_MESSAGE_HEADER = {'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01}; static constexpr std::chrono::milliseconds TCP_IP_CONNECTION_TIMEOUT_MS{3000}; // receive buffer size for each async_receive call. Larger than the // protocol's per-message cap (256 bytes) by a wide margin static constexpr size_t RECEIVE_BUFFER_SIZE = 65536; - static constexpr int BITS_PER_BYTE = 8; - - // axiomatic Protocol Message Header layout (per spec v6, Table 1): - // bytes 0-3 : Axiomatic Tag "AXIO" - // bytes 4-5 : Protocol ID (0x36BA, LSB first → 0xBA 0x36 on wire) - // bytes 6-7 : Message ID (LSB first) - // byte 8 : Message Version - // bytes 9-10 : Message Data Length (LSB first) - // total header = 11 bytes; Message Data follows immediately after. - static constexpr size_t PROTOCOL_HEADER_BYTES = 11; - - // the first 6 bytes of every Axiomatic header are constant ("AXIO" + - // protocol ID). Useful when we only want to validate "this is some - // axiomatic protocol message" without dictating Message ID. - static constexpr size_t PROTOCOL_SYNC_PREFIX_BYTES = 6; - - // byte offset of the Message ID field (LSB) within a protocol header. - // message ID is a 2-byte little-endian value at offsets 6 and 7. - static constexpr size_t MESSAGE_ID_HEADER_OFFSET = 6; - - // byte offset of the Message Data Length field (LSB) within a protocol - // header. 2-byte little-endian value at offsets 9 and 10. Names the body - // length, not the total message length. - static constexpr size_t MESSAGE_DATA_LENGTH_HEADER_OFFSET = 9; - - // message IDs defined by the Axiomatic Communication Protocol (spec v6, table 3). - enum class MessageId : uint16_t - { - Undefined = 0, - CanStream = 1, - StatusRequest = 2, - StatusResponse = 3, - Heartbeat = 4, - CanFdStream = 5, - }; - - // CAN ID field width inside a CAN Stream message body - static constexpr size_t STANDARD_CAN_ID_BYTES = 2; - static constexpr size_t EXTENDED_CAN_ID_BYTES = 4; - - // the Control Byte is a single byte at the start of every CAN/Notification - // frame in a CAN Stream message body - static constexpr size_t CONTROL_BYTE_BYTES = 1; - - // control byte (CB) field layout — first byte of every CAN/Notification - // frame inside a CAN Stream message body. Per Axiomatic Communication - // protocol spec v6, Section "Control Byte": - // bit 7 : C_Bit — 0 = CAN Frame, 1 = Notification Frame - // bits 6:5 : TS_Bit — Time Stamp length code (see TIMESTAMP_LENGTH_BYTES_TABLE) - // bit 4 : EID_Bit — 0 = standard 11-bit ID, 1 = extended 29-bit ID - // bits 3:0 : L_Bit — CAN Data Length (DLC), 0..8 valid - static constexpr uint8_t CONTROL_BYTE_NOTIFICATION_FRAME_FLAG = 0x80; - static constexpr uint8_t CONTROL_BYTE_TIMESTAMP_LENGTH_MASK = 0x60; - static constexpr int CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT = 5; - static constexpr uint8_t CONTROL_BYTE_EXTENDED_ID_FLAG = 0x10; - static constexpr uint8_t CONTROL_BYTE_CAN_DATA_LENGTH_MASK = 0x0F; - - // TS_Bit code → number of timestamp bytes that follow CB. Per Table 4 in - // the spec: 00→0 bytes, 01→1, 10→2, 11→4. Note this mapping is non-linear - // at index 3 (which is why a lookup table is needed instead of using the - // raw 2-bit value as the byte count directly). - static constexpr size_t TIMESTAMP_LENGTH_BYTES_TABLE[4] = {0, 1, 2, 4}; - - // TS_Bit code that send() writes into the Control Byte - static constexpr uint8_t CONTROL_BYTE_TIMESTAMP_2_BYTE_VALUE = static_cast(2) - << CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT; - - // notification Frame fixed size: 1-byte NIDB + 4-byte NDB1..NDB4. - static constexpr size_t NOTIFICATION_FRAME_TOTAL_BYTES = 5; /// @brief socket connection state as a struct for the mutex during TCP Open Socket to update the variables together struct TCPSocketConnectionState @@ -521,7 +324,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl std::atomic thread_running_; std::atomic stop_thread_requested_; - // CAN frames decoded from a packed CAN Stream message but not yet delivered + // CAN frames decoded from one TCP read but not yet delivered // through receive(). Drained one at a time, ahead of the next TCP read. std::deque pending_frames_; @@ -537,6 +340,8 @@ class AxiomaticAdapter::AxiomaticAdapterImpl // when true, disable Nagle's algorithm on the TCP socket after connect bool tcp_nodelay_; + + std::unique_ptr model_; }; AxiomaticAdapter::AxiomaticAdapter( @@ -545,14 +350,16 @@ AxiomaticAdapter::AxiomaticAdapter( const std::function frame)> && receive_callback_function, const std::function && error_callback_function, const std::chrono::milliseconds & receive_timeout_ms, - bool tcp_nodelay) + bool tcp_nodelay, + std::unique_ptr model) : pimpl_(std::make_unique( ip_address, port, std::move(receive_callback_function), std::move(error_callback_function), receive_timeout_ms, - tcp_nodelay)) + tcp_nodelay, + std::move(model))) {} AxiomaticAdapter::~AxiomaticAdapter() @@ -612,5 +419,4 @@ bool AxiomaticAdapter::is_thread_running() return pimpl_->is_thread_running(); } -} // namespace can -} // namespace polymath +} // namespace polymath::can diff --git a/axiomatic_adapter/src/axiomatic_model.cpp b/axiomatic_adapter/src/axiomatic_model.cpp new file mode 100644 index 0000000..b36abfe --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_model.cpp @@ -0,0 +1,59 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "axiomatic_adapter/axiomatic_model.hpp" + +#include +#include +#include +#include +#include + +#include "axiomatic_adapter/models/ax140900.hpp" +#include "axiomatic_adapter/models/ax142100a.hpp" + +namespace polymath::can +{ + +protocol::DecodeResult AxiomaticModel::decode(const uint8_t * data, size_t size) const +{ + protocol::ParsedMessages parsed = protocol::parseMessages(protocolId(), data, size); + protocol::DecodeResult result{{}, std::move(parsed.diagnostics)}; + for (const auto & message : parsed.messages) { + if (!decodeMessage(message, result)) { + result.diagnostics.push_back( + "SKIP: Message ID " + std::to_string(message.message_id) + " (" + std::to_string(message.body_size) + + "-byte body) carries no CAN frames"); + } + } + return result; +} + +std::unique_ptr makeModel(const std::string & model) +{ + if ("ax140900" == model) { + return std::make_unique(); + } + if ("ax142100a" == model) { + return std::make_unique(); + } + throw std::invalid_argument("Unknown Axiomatic model \"" + model + "\""); +} + +std::vector modelNames() +{ + return {"ax140900", "ax142100a"}; +} + +} // namespace polymath::can diff --git a/axiomatic_adapter/src/axiomatic_protocol.cpp b/axiomatic_adapter/src/axiomatic_protocol.cpp new file mode 100644 index 0000000..681c8ec --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_protocol.cpp @@ -0,0 +1,116 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "axiomatic_adapter/axiomatic_protocol.hpp" + +#include +#include +#include +#include + +namespace polymath::can::protocol +{ + +namespace +{ +constexpr int BITS_PER_BYTE = 8; +constexpr size_t PROTOCOL_ID_BYTES = 2; +constexpr size_t MESSAGE_DATA_LENGTH_BYTES = 2; +constexpr size_t MESSAGE_ID_BYTES = 2; +} // namespace + +uint32_t readLittleEndian(const uint8_t * data, size_t num_bytes) +{ + uint32_t value = 0; + for (size_t i = 0; i < num_bytes; ++i) { + value |= static_cast(data[i]) << (BITS_PER_BYTE * i); + } + return value; +} + +void appendLittleEndian(std::vector & out, uint32_t value, size_t num_bytes) +{ + for (size_t i = 0; i < num_bytes; ++i) { + out.push_back(static_cast((value >> (BITS_PER_BYTE * i)) & 0xFF)); + } +} + +std::vector encodeMessage( + uint16_t protocol_id, uint16_t message_id, uint8_t message_version, const std::vector & body) +{ + std::vector message(AXIOMATIC_TAG.begin(), AXIOMATIC_TAG.end()); + message.reserve(HEADER_BYTES + body.size()); + appendLittleEndian(message, protocol_id, PROTOCOL_ID_BYTES); + appendLittleEndian(message, message_id, MESSAGE_ID_BYTES); + message.push_back(message_version); + appendLittleEndian(message, static_cast(body.size()), MESSAGE_DATA_LENGTH_BYTES); + message.insert(message.end(), body.begin(), body.end()); + return message; +} + +ParsedMessages parseMessages(uint16_t protocol_id, const uint8_t * data, size_t size) +{ + ParsedMessages parsed; + if (size < HEADER_BYTES) { + parsed.diagnostics.push_back( + "DROP: received " + std::to_string(size) + " bytes, too short to contain a complete protocol header"); + return parsed; + } + + size_t scan_pos = 0; + while (scan_pos + HEADER_BYTES <= size) { + const uint8_t * header = data + scan_pos; + const bool tag_match = std::equal(AXIOMATIC_TAG.begin(), AXIOMATIC_TAG.end(), header); + const uint16_t received_protocol_id = + static_cast(readLittleEndian(header + PROTOCOL_ID_OFFSET, PROTOCOL_ID_BYTES)); + if (!tag_match || protocol_id != received_protocol_id) { + std::ostringstream message; + message << "DROP: sync prefix mismatch at offset " << scan_pos << " of " << size + << "-byte read; bytes there:" << std::hex; + for (size_t i = 0; i < PROTOCOL_ID_OFFSET + PROTOCOL_ID_BYTES; ++i) { + message << ' ' << static_cast(header[i]); + } + message << std::dec << " (expected Protocol ID 0x" << std::hex << protocol_id << std::dec << "; remaining " + << (size - scan_pos) << " bytes ignored)"; + if (tag_match) { + message << "; the device speaks a different Protocol ID, so the wrong --model may be selected"; + } + parsed.diagnostics.push_back(message.str()); + return parsed; + } + + const size_t declared_length = readLittleEndian(header + MESSAGE_DATA_LENGTH_OFFSET, MESSAGE_DATA_LENGTH_BYTES); + const size_t body_start = scan_pos + HEADER_BYTES; + const size_t body_end = std::min(body_start + declared_length, size); + if (body_start + declared_length > size) { + parsed.diagnostics.push_back( + "TRUNCATED: message at offset " + std::to_string(scan_pos) + " declares " + std::to_string(declared_length) + + " body bytes but only " + std::to_string(body_end - body_start) + " were received"); + } + parsed.messages.push_back(MessageView{ + static_cast(readLittleEndian(header + MESSAGE_ID_OFFSET, MESSAGE_ID_BYTES)), + header[MESSAGE_VERSION_OFFSET], + data + body_start, + body_end - body_start}); + scan_pos = body_start + declared_length; + } + + if (scan_pos < size) { + parsed.diagnostics.push_back( + "DROP: " + std::to_string(size - scan_pos) + " trailing bytes too short to contain a protocol header"); + } + return parsed; +} + +} // namespace polymath::can::protocol diff --git a/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp index 01630e5..f7198ac 100644 --- a/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp +++ b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp @@ -15,11 +15,11 @@ #include "axiomatic_adapter/axiomatic_socketcan_bridge.hpp" #include +#include #include +#include -namespace polymath -{ -namespace can +namespace polymath::can { AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( @@ -27,7 +27,8 @@ AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( const std::string & ip, const std::string & port, bool verbose, - bool tcp_nodelay) + bool tcp_nodelay, + std::unique_ptr model) : socketcan_adapter_(can_interface_name) , axiomatic_adapter_( ip, @@ -35,7 +36,8 @@ AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( std::bind(&AxiomaticSocketcanBridge::ethcanReceiveCallback, this, std::placeholders::_1), [](AxiomaticAdapter::socket_error_string_t /*error*/) { /* no-op */ }, AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, - tcp_nodelay) + tcp_nodelay, + std::move(model)) , verbose_(verbose) { socketcan_adapter_.setOnReceiveCallback( @@ -143,5 +145,4 @@ void AxiomaticSocketcanBridge::ethcanReceiveCallback(std::unique_ptrdefault_val("vcan0"); app.add_option("ip", ip, "IP address of the bridge (default: 192.168.0.34)")->default_val("192.168.0.34"); @@ -63,6 +64,9 @@ void configureArguments( "!--no-tcp-nodelay", tcp_nodelay, "Disable TCP_NODELAY (re-enable Nagle's algorithm). Default is on; see the AxiomaticAdapter docs for tradeoffs."); + app.add_option("-m,--model", model, "Axiomatic converter model (default: ax140900)") + ->transform(CLI::IsMember(polymath::can::modelNames(), CLI::ignore_case)) + ->default_val("ax140900"); } int main(int argc, char * argv[]) @@ -71,13 +75,15 @@ int main(int argc, char * argv[]) std::signal(SIGTERM, signalHandler); std::string can_interface, ip, port; bool tcp_nodelay = true; + std::string model; CLI::App app{"Axiomatic SocketCAN Bridge"}; - configureArguments(app, can_interface, ip, port, verbose, retry_connection, max_retry_attempts, tcp_nodelay); + configureArguments(app, can_interface, ip, port, verbose, retry_connection, max_retry_attempts, tcp_nodelay, model); CLI11_PARSE(app, argc, argv); - polymath::can::AxiomaticSocketcanBridge bridge(can_interface, ip, port, verbose, tcp_nodelay); + polymath::can::AxiomaticSocketcanBridge bridge( + can_interface, ip, port, verbose, tcp_nodelay, polymath::can::makeModel(model)); - std::cout << "Axiomatic Socketcan Bridge configuring..." << std::endl; + std::cout << "Axiomatic Socketcan Bridge configuring with model " << model << "..." << std::endl; while (!shutdown_requested.load() && !bridge.on_configure()) { std::cerr << "Configuration failed."; diff --git a/axiomatic_adapter/src/models/ax140900.cpp b/axiomatic_adapter/src/models/ax140900.cpp new file mode 100644 index 0000000..11e6a06 --- /dev/null +++ b/axiomatic_adapter/src/models/ax140900.cpp @@ -0,0 +1,144 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "axiomatic_adapter/models/ax140900.hpp" + +#include +#include +#include +#include +#include + +namespace polymath::can::ax140900 +{ + +namespace +{ +constexpr uint8_t MESSAGE_VERSION = 0; + +constexpr size_t STANDARD_CAN_ID_BYTES = 2; +constexpr size_t EXTENDED_CAN_ID_BYTES = 4; +constexpr size_t CONTROL_BYTE_BYTES = 1; + +// Control Byte (CB), first byte of every CAN or Notification frame in a CAN Stream body: +// bit 7 : C_Bit — 0 = CAN Frame, 1 = Notification Frame +// bits 6:5 : TS_Bit — Time Stamp length code, see TIMESTAMP_LENGTH_BYTES_TABLE +// bit 4 : EID_Bit — 0 = standard 11-bit ID, 1 = extended 29-bit ID +// bits 3:0 : L_Bit — CAN Data Length, 0..8 +constexpr uint8_t CONTROL_BYTE_NOTIFICATION_FRAME_FLAG = 0x80; +constexpr uint8_t CONTROL_BYTE_TIMESTAMP_LENGTH_MASK = 0x60; +constexpr int CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT = 5; +constexpr uint8_t CONTROL_BYTE_EXTENDED_ID_FLAG = 0x10; +constexpr uint8_t CONTROL_BYTE_CAN_DATA_LENGTH_MASK = 0x0F; + +// TS_Bit code → time stamp bytes following CB (spec table 4); code 3 is 4 bytes, not 3 +constexpr std::array TIMESTAMP_LENGTH_BYTES_TABLE = {0, 1, 2, 4}; + +// encode() always writes a 2-byte time stamp of 0x46C0 +constexpr uint8_t SEND_TIMESTAMP_CODE = 2; +constexpr uint16_t SEND_TIMESTAMP = 0x46C0; +constexpr size_t SEND_TIMESTAMP_BYTES = 2; + +// Notification Frame: 1-byte NIDB + 4-byte NDB1..NDB4 +constexpr size_t NOTIFICATION_FRAME_TOTAL_BYTES = 5; + +std::string hexByte(uint8_t value) +{ + std::ostringstream stream; + stream << "0x" << std::hex << static_cast(value); + return stream.str(); +} + +/// Appends every CAN frame in one CAN Stream message body to result +void decodeCanStream(const uint8_t * body, size_t body_size, protocol::DecodeResult & result) +{ + size_t walker = 0; + while (walker + CONTROL_BYTE_BYTES <= body_size) { + const uint8_t control_byte = body[walker]; + if (0 != (control_byte & CONTROL_BYTE_NOTIFICATION_FRAME_FLAG)) { + result.diagnostics.push_back( + "SKIP: notification frame (CB=" + hexByte(control_byte) + ") at body offset " + std::to_string(walker)); + walker += NOTIFICATION_FRAME_TOTAL_BYTES; + continue; + } + const size_t timestamp_size = TIMESTAMP_LENGTH_BYTES_TABLE + [(control_byte & CONTROL_BYTE_TIMESTAMP_LENGTH_MASK) >> CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT]; + const bool extended_id = 0 != (control_byte & CONTROL_BYTE_EXTENDED_ID_FLAG); + const size_t id_size = extended_id ? EXTENDED_CAN_ID_BYTES : STANDARD_CAN_ID_BYTES; + const size_t can_data_length = control_byte & CONTROL_BYTE_CAN_DATA_LENGTH_MASK; + const size_t frame_bytes = CONTROL_BYTE_BYTES + timestamp_size + id_size + can_data_length; + if (walker + frame_bytes > body_size) { + result.diagnostics.push_back( + "DROP: truncated CAN frame at body offset " + std::to_string(walker) + " (CB=" + hexByte(control_byte) + + " declares " + std::to_string(frame_bytes) + " bytes but only " + std::to_string(body_size - walker) + + " remain)"); + return; + } + const uint8_t * id_start = body + walker + CONTROL_BYTE_BYTES + timestamp_size; + std::array data_bytes = {0}; + /// TODO: (David Tarazi) Drop frames whose CAN data length exceeds CAN_MAX_DLC; up to 15 overflows data_bytes. + std::copy_n(id_start + id_size, can_data_length, data_bytes.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(protocol::readLittleEndian(id_start, id_size)); + frame.set_len(static_cast(can_data_length)); + frame.set_data(data_bytes); + if (extended_id) { + frame.set_id_as_extended(); + } + result.frames.push_back(frame); + walker += frame_bytes; + } +} +} // namespace + +std::vector Ax140900::encode(const polymath::socketcan::CanFrame & frame) const +{ + const bool extended_id = polymath::socketcan::IdType::EXTENDED == frame.get_id_type(); + const size_t data_length = frame.get_len(); + const auto data = frame.get_data(); + + std::vector body; + body.push_back(static_cast( + (SEND_TIMESTAMP_CODE << CONTROL_BYTE_TIMESTAMP_LENGTH_SHIFT) | (extended_id ? CONTROL_BYTE_EXTENDED_ID_FLAG : 0) | + (data_length & CONTROL_BYTE_CAN_DATA_LENGTH_MASK))); + protocol::appendLittleEndian(body, SEND_TIMESTAMP, SEND_TIMESTAMP_BYTES); + protocol::appendLittleEndian(body, frame.get_id(), extended_id ? EXTENDED_CAN_ID_BYTES : STANDARD_CAN_ID_BYTES); + const size_t declared_body_size = body.size() + data_length; + body.insert(body.end(), data.begin(), data.end()); + + std::vector message = + protocol::encodeMessage(PROTOCOL_ID, static_cast(MessageId::CanStream), MESSAGE_VERSION, body); + /// TODO: (David Tarazi) Send only the frame's data length in bytes; the header declares that many, but all + /// CAN_MAX_DLC data bytes are sent. + message[protocol::MESSAGE_DATA_LENGTH_OFFSET] = static_cast(declared_body_size & 0xFF); + message[protocol::MESSAGE_DATA_LENGTH_OFFSET + 1] = static_cast((declared_body_size >> 8) & 0xFF); + return message; +} + +uint16_t Ax140900::protocolId() const +{ + return PROTOCOL_ID; +} + +bool Ax140900::decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const +{ + if (static_cast(MessageId::CanStream) != message.message_id) { + return false; + } + decodeCanStream(message.body, message.body_size, result); + return true; +} + +} // namespace polymath::can::ax140900 diff --git a/axiomatic_adapter/src/models/ax142100a.cpp b/axiomatic_adapter/src/models/ax142100a.cpp new file mode 100644 index 0000000..7091934 --- /dev/null +++ b/axiomatic_adapter/src/models/ax142100a.cpp @@ -0,0 +1,116 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "axiomatic_adapter/models/ax142100a.hpp" + +#include +#include +#include +#include + +namespace polymath::can::ax142100a +{ + +namespace +{ +constexpr uint8_t MESSAGE_VERSION = 0; + +/// TODO: (David) Confirm the 11-bit ID width on hardware; UMAX142100A only shows a 29-bit example. +constexpr size_t STANDARD_CAN_ID_BYTES = 2; +constexpr size_t EXTENDED_CAN_ID_BYTES = 4; +constexpr size_t STATUS_BYTE_BYTES = 1; + +// Status byte, first byte of the Forwarded Data payload: +// bit 6 : raw data — the rest of the payload is unformatted serial/Ethernet data +// bit 4 : 0 = standard 11-bit ID, 1 = extended 29-bit ID +// bits 3:0 : CAN data length +constexpr uint8_t STATUS_BYTE_RAW_DATA_FLAG = 0x40; +constexpr uint8_t STATUS_BYTE_EXTENDED_ID_FLAG = 0x10; +constexpr uint8_t STATUS_BYTE_CAN_DATA_LENGTH_MASK = 0x0F; + +/// Appends every CAN frame in one Forwarded Data message body to result +void decodeForwardedData(const uint8_t * body, size_t body_size, protocol::DecodeResult & result) +{ + size_t walker = 0; + while (walker + STATUS_BYTE_BYTES <= body_size) { + const uint8_t status_byte = body[walker]; + if (0 != (status_byte & STATUS_BYTE_RAW_DATA_FLAG)) { + result.diagnostics.push_back( + "SKIP: raw data payload of " + std::to_string(body_size - walker) + " bytes at body offset " + + std::to_string(walker)); + return; + } + const bool extended_id = 0 != (status_byte & STATUS_BYTE_EXTENDED_ID_FLAG); + const size_t id_size = extended_id ? EXTENDED_CAN_ID_BYTES : STANDARD_CAN_ID_BYTES; + const size_t can_data_length = status_byte & STATUS_BYTE_CAN_DATA_LENGTH_MASK; + if (can_data_length > CAN_MAX_DLC) { + result.diagnostics.push_back( + "DROP: CAN data length " + std::to_string(can_data_length) + " at body offset " + std::to_string(walker) + + " exceeds " + std::to_string(CAN_MAX_DLC) + "; remainder of body dropped"); + return; + } + const size_t frame_bytes = STATUS_BYTE_BYTES + id_size + can_data_length; + if (walker + frame_bytes > body_size) { + result.diagnostics.push_back( + "DROP: truncated CAN frame at body offset " + std::to_string(walker) + " (declares " + + std::to_string(frame_bytes) + " bytes but only " + std::to_string(body_size - walker) + " remain)"); + return; + } + const uint8_t * id_start = body + walker + STATUS_BYTE_BYTES; + std::array data_bytes = {0}; + std::copy_n(id_start + id_size, can_data_length, data_bytes.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(protocol::readLittleEndian(id_start, id_size)); + frame.set_len(static_cast(can_data_length)); + frame.set_data(data_bytes); + if (extended_id) { + frame.set_id_as_extended(); + } + result.frames.push_back(frame); + walker += frame_bytes; + } +} +} // namespace + +std::vector Ax142100a::encode(const polymath::socketcan::CanFrame & frame) const +{ + const bool extended_id = polymath::socketcan::IdType::EXTENDED == frame.get_id_type(); + const size_t data_length = std::min(frame.get_len(), CAN_MAX_DLC); + const auto data = frame.get_data(); + + std::vector body; + body.push_back(static_cast( + (extended_id ? STATUS_BYTE_EXTENDED_ID_FLAG : 0) | (data_length & STATUS_BYTE_CAN_DATA_LENGTH_MASK))); + protocol::appendLittleEndian(body, frame.get_id(), extended_id ? EXTENDED_CAN_ID_BYTES : STANDARD_CAN_ID_BYTES); + body.insert(body.end(), data.begin(), data.begin() + data_length); + + return protocol::encodeMessage(PROTOCOL_ID, static_cast(MessageId::ForwardedData), MESSAGE_VERSION, body); +} + +uint16_t Ax142100a::protocolId() const +{ + return PROTOCOL_ID; +} + +bool Ax142100a::decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const +{ + if (static_cast(MessageId::ForwardedData) != message.message_id) { + return false; + } + decodeForwardedData(message.body, message.body_size, result); + return true; +} + +} // namespace polymath::can::ax142100a diff --git a/axiomatic_adapter/test/axiomatic_model_test.cpp b/axiomatic_adapter/test/axiomatic_model_test.cpp new file mode 100644 index 0000000..7ad96c1 --- /dev/null +++ b/axiomatic_adapter/test/axiomatic_model_test.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() + #include // v3 +#else + #include // v2 +#endif +#include "axiomatic_adapter/axiomatic_adapter.hpp" +#include "axiomatic_adapter/axiomatic_model.hpp" +#include "axiomatic_adapter/models/ax140900.hpp" +#include "axiomatic_adapter/models/ax142100a.hpp" + +using polymath::socketcan::CanFrame; +using polymath::socketcan::IdType; + +namespace +{ + +CanFrame makeFrame(canid_t id, bool extended, const std::vector & payload) +{ + std::array data = {0}; + std::copy(payload.begin(), payload.end(), data.begin()); + CanFrame frame; + frame.set_can_id(id); + if (extended) { + frame.set_id_as_extended(); + } + frame.set_len(static_cast(payload.size())); + frame.set_data(data); + return frame; +} + +void requireSameFrame(const CanFrame & actual, const CanFrame & expected) +{ + REQUIRE(actual.get_id_type() == expected.get_id_type()); + REQUIRE(actual.get_id() == expected.get_id()); + REQUIRE(actual.get_len() == expected.get_len()); + REQUIRE(actual.get_data() == expected.get_data()); +} + +std::vector concat(const std::vector & a, const std::vector & b) +{ + std::vector out(a); + out.insert(out.end(), b.begin(), b.end()); + return out; +} + +// UMAX142100A Figure 3: ID 0x18F00401, 29-bit, 8 data bytes +const std::vector AX142100A_MANUAL_EXAMPLE = { + 0x41, 0x58, 0x49, 0x4F, 0x28, 0x4E, 0x01, 0x00, 0x00, 0x0D, 0x00, 0x18, + 0x01, 0x04, 0xF0, 0x18, 0x12, 0x44, 0x21, 0x55, 0x61, 0x09, 0x01, 0x02, +}; +const CanFrame AX142100A_MANUAL_FRAME = makeFrame(0x18F00401, true, {0x12, 0x44, 0x21, 0x55, 0x61, 0x09, 0x01, 0x02}); + +} // namespace + +TEST_CASE("AX142100A matches the manual example", "[model][ax142100a]") +{ + SECTION("decode") + { + const auto result = + polymath::can::ax142100a::Ax142100a().decode(AX142100A_MANUAL_EXAMPLE.data(), AX142100A_MANUAL_EXAMPLE.size()); + REQUIRE(result.diagnostics.empty()); + REQUIRE(1 == result.frames.size()); + requireSameFrame(result.frames[0], AX142100A_MANUAL_FRAME); + } + + SECTION("encode") + { + REQUIRE(AX142100A_MANUAL_EXAMPLE == polymath::can::ax142100a::Ax142100a().encode(AX142100A_MANUAL_FRAME)); + } +} + +TEST_CASE("AX142100A skips raw data payloads", "[model][ax142100a]") +{ + const std::vector raw = { + 0x41, 0x58, 0x49, 0x4F, 0x28, 0x4E, 0x01, 0x00, 0x00, 0x04, 0x00, 0x40, 'a', 'b', 'c'}; + const auto stream = concat(raw, AX142100A_MANUAL_EXAMPLE); + + const auto result = polymath::can::ax142100a::Ax142100a().decode(stream.data(), stream.size()); + REQUIRE(1 == result.diagnostics.size()); + REQUIRE(1 == result.frames.size()); + requireSameFrame(result.frames[0], AX142100A_MANUAL_FRAME); +} + +TEST_CASE("AX140900 encodes a CAN Stream message with all CAN_MAX_DLC data bytes", "[model][ax140900]") +{ + const std::vector expected = { + 'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01, 0x00, 0x00, 0x0B, 0x00, // header, 11-byte body + 0x54, // CB: 2-byte TS, extended, DLC 4 + 0xC0, 0x46, // time stamp + 0x01, 0x04, 0xF0, 0x18, // ID + 0x01, 0x02, 0x03, 0x04, 0x00, 0x00, 0x00, 0x00, // data: 4 declared, 4 past the body length + }; + REQUIRE( + expected == polymath::can::ax140900::Ax140900().encode(makeFrame(0x18F00401, true, {0x01, 0x02, 0x03, 0x04}))); +} + +TEST_CASE("AX140900 decodes packed frames and skips non-CAN content", "[model][ax140900]") +{ + const std::vector can_stream = { + 'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01, 0x00, 0x00, 0x15, 0x00, // CAN Stream, 21-byte body + 0x02, 0x23, 0x01, 0xAA, 0xBB, // no TS, standard 0x123, DLC 2 + 0x80, 0x00, 0x00, 0x00, 0x00, // notification frame + 0x71, 0x01, 0x02, 0x03, 0x04, 0x01, 0x00, 0xF0, 0x18, 0xCC, // 4-byte TS, extended 0x18F00001, DLC 1 + 0x01, // truncated: DLC 1 frame with no ID + }; + const std::vector heartbeat = {'A', 'X', 'I', 'O', 0xBA, 0x36, 0x04, 0x00, 0x00, 0x00, 0x00}; + const auto stream = concat(can_stream, heartbeat); + + const auto result = polymath::can::ax140900::Ax140900().decode(stream.data(), stream.size()); + REQUIRE(2 == result.frames.size()); + requireSameFrame(result.frames[0], makeFrame(0x123, false, {0xAA, 0xBB})); + requireSameFrame(result.frames[1], makeFrame(0x18F00001, true, {0xCC})); + // notification, truncated frame, heartbeat + REQUIRE(3 == result.diagnostics.size()); +} + +TEST_CASE("Models round trip", "[model]") +{ + const std::vector frames = { + makeFrame(0x123, false, {0x01, 0x02, 0x03}), + makeFrame(0x18F00401, true, {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08}), + makeFrame(0x7FF, false, {}), + }; + + for (const auto & name : polymath::can::modelNames()) { + INFO("model " << name); + const auto model = polymath::can::makeModel(name); + for (const auto & frame : frames) { + const auto message = model->encode(frame); + const auto result = model->decode(message.data(), message.size()); + REQUIRE(1 == result.frames.size()); + requireSameFrame(result.frames[0], frame); + } + } +} + +TEST_CASE("Models reject another model's Protocol ID", "[model]") +{ + const auto result = + polymath::can::ax140900::Ax140900().decode(AX142100A_MANUAL_EXAMPLE.data(), AX142100A_MANUAL_EXAMPLE.size()); + REQUIRE(result.frames.empty()); + REQUIRE(1 == result.diagnostics.size()); + REQUIRE(std::string::npos != result.diagnostics[0].find("wrong --model")); +} + +TEST_CASE("Models drop buffers shorter than a header", "[model]") +{ + const std::vector partial(AX142100A_MANUAL_EXAMPLE.begin(), AX142100A_MANUAL_EXAMPLE.begin() + 5); + const auto result = polymath::can::ax142100a::Ax142100a().decode(partial.data(), partial.size()); + REQUIRE(result.frames.empty()); + REQUIRE(1 == result.diagnostics.size()); +} + +TEST_CASE("Model names map to their classes", "[model]") +{ + REQUIRE(2 == polymath::can::modelNames().size()); + REQUIRE( + nullptr != dynamic_cast(polymath::can::makeModel("ax140900").get())); + REQUIRE( + nullptr != dynamic_cast(polymath::can::makeModel("ax142100a").get())); + REQUIRE_THROWS_AS(polymath::can::makeModel("bogus"), std::invalid_argument); +} + +TEST_CASE("AxiomaticAdapter rejects a null model", "[model]") +{ + REQUIRE_THROWS_AS( + polymath::can::AxiomaticAdapter( + "192.168.0.34", + "4000", + [](std::unique_ptr /*frame*/) {}, + [](polymath::can::AxiomaticAdapter::socket_error_string_t /*error*/) {}, + polymath::can::AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, + true, + nullptr), + std::invalid_argument); +}