From bfcde5d96e676426b83db85f5072bcd18eeb9f68 Mon Sep 17 00:00:00 2001 From: Eric McCarter Date: Wed, 20 May 2026 16:35:52 +0000 Subject: [PATCH 1/2] first pass at hpo handling, sygnal_interface_socketcan and ROS2 node implementation deffered to a separate commit --- .../sygnal_can_interface_lib/CMakeLists.txt | 17 + .../sygnal_hpo_interface.hpp | 133 +++++++ .../src/sygnal_hpo_interface.cpp | 254 ++++++++++++ .../test/sygnal_hpo_interface_test.cpp | 371 ++++++++++++++++++ 4 files changed, 775 insertions(+) create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt index 5d3fad8..f7ba09b 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -32,6 +32,7 @@ add_library( src/crc8.cpp src/sygnal_mcm_interface.cpp src/sygnal_command_interface.cpp + src/sygnal_hpo_interface.cpp src/sygnal_interface_socketcan.cpp ) @@ -108,6 +109,22 @@ if(BUILD_TESTING) WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ) + add_executable(sygnal_hpo_interface_tests + test/sygnal_hpo_interface_test.cpp + ) + target_link_libraries(sygnal_hpo_interface_tests + PRIVATE ${PROJECT_NAME} sygnal_dbc::sygnal_dbc Catch2::Catch2WithMain + ) + ament_add_test( + sygnal_hpo_interface_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + -r junit -s + -o test_results/${PROJECT_NAME}/sygnal_hpo_interface_tests_output.xml + ENV CATCH_CONFIG_CONSOLE_WIDTH=120 + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ) + if(DEFINED ENV{CAN_AVAILABLE}) add_executable(sygnal_interface_socketcan_tests test/sygnal_interface_socketcan_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp new file mode 100644 index 0000000..7a58a3e --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp @@ -0,0 +1,133 @@ +// 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 SYGNAL_CAN_INTERFACE_LIB__SYGNAL_HPO_INTERFACE_HPP_ +#define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_HPO_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "socketcan_adapter/can_frame.hpp" + +namespace polymath::sygnal +{ + +constexpr uint8_t HPO_NUM_INTERFACES = 7; + +/// @brief Parsed HPO ControlEnableResponse / ControlCommandResponse. +/// is_enable_response disambiguates which payload is meaningful: +/// when true, the `enable` field carries the response; when false, +/// the `value` field carries the float command response. +struct HpoControlResponse +{ + uint8_t message_id; + uint8_t bus_address; + double value; + bool enable; + bool is_enable_response; +}; + +/// @brief Parsed HPO ErrorStatus frame fields. +struct HpoErrorStatus +{ + uint8_t bus_address; + uint8_t subsystem_id; + uint8_t error_type; + uint16_t error_can_id; + uint8_t config_section_id; + uint8_t config_interface_id; +}; + +/// @brief Self-contained HPO board representation. +/// +/// Owns the per-device interface bits (7 per-interface + 1 overall) and produces / +/// consumes the CAN frames the HPO understands. Frames are routed to the right +/// instance by `bus_address_`: every parse helper rejects frames that do not match +/// this device's bus address. Command frames are populated with `bus_address_` +/// automatically so callers cannot mismatch addresses. +/// +/// Unlike the MCM, the HPO does not hold a Sygnal state machine. The per-interface +/// and overall-interface heartbeat signals are 1-bit booleans (true = HPO in control, +/// false = released). The MCM's SystemState byte is not tracked here. +class SygnalHpoInterface +{ +public: + SygnalHpoInterface(); + explicit SygnalHpoInterface(uint8_t bus_address); + ~SygnalHpoInterface() = default; + + /// @brief Try to parse a CAN frame as an HPO heartbeat addressed to this device. + /// @param frame Raw CAN frame. + /// @return true if the frame matched and internal state was updated. + bool parseHeartbeatFrame(const socketcan::CanFrame & frame); + + /// @brief Try to parse a CAN frame as a ControlEnableResponse or ControlCommandResponse + /// addressed to this device. + /// @param frame Raw CAN frame. + /// @return Populated HpoControlResponse on success, std::nullopt otherwise. + std::optional parseControlResponse(const socketcan::CanFrame & frame); + + /// @brief Try to parse a CAN frame as an ErrorStatus addressed to this device. + /// @param frame Raw CAN frame. + /// @return Populated HpoErrorStatus on success, std::nullopt otherwise. + std::optional parseErrorFrame(const socketcan::CanFrame & frame); + + /// @brief Build a ControlEnable frame for this device. + /// @param message_id 8-bit HPO MessageID. + /// @param enable true to grant HPO control, false to release. + /// @param[out] error_message Populated on failure. + /// @return Packed CAN frame on success. + std::optional createControlEnableFrame( + uint8_t message_id, bool enable, std::string & error_message); + + /// @brief Build a ControlCommand frame for this device. + /// @param message_id 8-bit HPO MessageID. + /// @param value Command value (encoded as float per the DBC). + /// @param[out] error_message Populated on failure. + /// @return Packed CAN frame on success. + std::optional createControlCommandFrame( + uint8_t message_id, double value, std::string & error_message); + + uint8_t get_bus_address() const + { + return bus_address_; + } + + std::array get_interface_states() const + { + return hpo_interface_states_; + } + + bool get_overall_interface_state() const + { + return hpo_overall_interface_state_; + } + + std::chrono::system_clock::time_point get_last_heartbeat_timestamp() const + { + return last_heartbeat_timestamp_; + } + +private: + uint8_t bus_address_; + std::array hpo_interface_states_; + bool hpo_overall_interface_state_; + std::chrono::system_clock::time_point last_heartbeat_timestamp_; +}; + +} // namespace polymath::sygnal + +#endif // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_HPO_INTERFACE_HPP_ diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp new file mode 100644 index 0000000..c675244 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp @@ -0,0 +1,254 @@ +// 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 "sygnal_can_interface_lib/sygnal_hpo_interface.hpp" + +#include +#include +#include + +#include "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_dbc/hpo_control.h" +#include "sygnal_dbc/hpo_error.h" +#include "sygnal_dbc/hpo_heartbeat.h" + +namespace polymath::sygnal +{ + +SygnalHpoInterface::SygnalHpoInterface() +: bus_address_(0) +, hpo_overall_interface_state_(false) +{ + hpo_interface_states_.fill(false); +} + +SygnalHpoInterface::SygnalHpoInterface(uint8_t bus_address) +: bus_address_(bus_address) +, hpo_overall_interface_state_(false) +{ + hpo_interface_states_.fill(false); +} + +bool SygnalHpoInterface::parseHeartbeatFrame(const socketcan::CanFrame & frame) +{ + if (HPO_HEARTBEAT_HEARTBEAT_FRAME_ID != frame.get_id()) { + return false; + } + + auto frame_copy = frame.get_frame(); + + if (!check_crc8(reinterpret_cast(frame_copy.data))) { + return false; + } + + hpo_heartbeat_heartbeat_t unpacked; + if (0 != hpo_heartbeat_heartbeat_init(&unpacked)) { + return false; + } + + if (0 != hpo_heartbeat_heartbeat_unpack(&unpacked, frame_copy.data, frame_copy.len)) { + return false; + } + + if (bus_address_ != unpacked.bus_address) { + return false; + } + + last_heartbeat_timestamp_ = std::chrono::system_clock::now(); + + hpo_interface_states_[0] = (0 != unpacked.interface0_state); + hpo_interface_states_[1] = (0 != unpacked.interface1_state); + hpo_interface_states_[2] = (0 != unpacked.interface2_state); + hpo_interface_states_[3] = (0 != unpacked.interface3_state); + hpo_interface_states_[4] = (0 != unpacked.interface4_state); + hpo_interface_states_[5] = (0 != unpacked.interface5_state); + hpo_interface_states_[6] = (0 != unpacked.interface6_state); + hpo_overall_interface_state_ = (0 != unpacked.overall_interface_state); + + return true; +} + +std::optional SygnalHpoInterface::parseControlResponse(const socketcan::CanFrame & frame) +{ + auto frame_copy = frame.get_frame(); + const uint32_t frame_id = frame.get_id(); + + if ( + HPO_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID != frame_id && + HPO_CONTROL_CONTROL_COMMAND_RESPONSE_FRAME_ID != frame_id) + { + return std::nullopt; + } + + if (!check_crc8(reinterpret_cast(frame_copy.data))) { + return std::nullopt; + } + + HpoControlResponse response{}; + + if (HPO_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID == frame_id) { + hpo_control_control_enable_response_t unpacked; + if (0 != hpo_control_control_enable_response_init(&unpacked)) { + return std::nullopt; + } + if (0 != hpo_control_control_enable_response_unpack(&unpacked, frame_copy.data, frame_copy.len)) { + return std::nullopt; + } + if (bus_address_ != unpacked.bus_address) { + return std::nullopt; + } + + response.is_enable_response = true; + response.bus_address = unpacked.bus_address; + response.message_id = unpacked.message_id; + response.enable = (0 != unpacked.enable); + response.value = 0.0; + return response; + } + + hpo_control_control_command_response_t unpacked; + if (0 != hpo_control_control_command_response_init(&unpacked)) { + return std::nullopt; + } + if (0 != hpo_control_control_command_response_unpack(&unpacked, frame_copy.data, frame_copy.len)) { + return std::nullopt; + } + if (bus_address_ != unpacked.bus_address) { + return std::nullopt; + } + + response.is_enable_response = false; + response.bus_address = unpacked.bus_address; + response.message_id = unpacked.message_id; + response.value = hpo_control_control_command_response_value_decode(unpacked.value); + response.enable = false; + return response; +} + +std::optional SygnalHpoInterface::parseErrorFrame(const socketcan::CanFrame & frame) +{ + if (HPO_ERROR_ERROR_STATUS_FRAME_ID != frame.get_id()) { + return std::nullopt; + } + + auto frame_copy = frame.get_frame(); + + if (!check_crc8(reinterpret_cast(frame_copy.data))) { + return std::nullopt; + } + + hpo_error_error_status_t unpacked; + if (0 != hpo_error_error_status_init(&unpacked)) { + return std::nullopt; + } + if (0 != hpo_error_error_status_unpack(&unpacked, frame_copy.data, frame_copy.len)) { + return std::nullopt; + } + if (bus_address_ != unpacked.bus_address) { + return std::nullopt; + } + + HpoErrorStatus status{}; + status.bus_address = unpacked.bus_address; + status.subsystem_id = unpacked.subsystem_id; + status.error_type = unpacked.error_type; + status.error_can_id = unpacked.error_canid; + status.config_section_id = unpacked.config_section_id; + status.config_interface_id = unpacked.config_interface_id; + return status; +} + +std::optional SygnalHpoInterface::createControlEnableFrame( + uint8_t message_id, bool enable, std::string & error_message) +{ + polymath::socketcan::CanFrame frame; + uint8_t buffer[CAN_MAX_DLC]; + hpo_control_control_enable_t unpacked; + + if (0 != hpo_control_control_enable_init(&unpacked)) { + error_message += "Unable to initialize hpo_control_control_enable_t. \n"; + return std::nullopt; + } + + unpacked.bus_address = bus_address_; + unpacked.message_id = message_id; + unpacked.enable = enable ? 1 : 0; + unpacked.crc = 0; + + if (!hpo_control_control_enable_pack(buffer, &unpacked, sizeof(buffer))) { + error_message += "Could not pack hpo_control_control_enable_t. \n"; + return std::nullopt; + } + + unpacked.crc = generate_crc8(buffer); + if (!hpo_control_control_enable_pack(buffer, &unpacked, sizeof(buffer))) { + error_message += "Could not pack hpo_control_control_enable_t. \n"; + return std::nullopt; + } + + std::array packed_data; + for (size_t i = 0; i < packed_data.size(); ++i) { + packed_data[i] = buffer[i]; + } + + frame.set_can_id(HPO_CONTROL_CONTROL_ENABLE_FRAME_ID); + frame.set_len(HPO_CONTROL_CONTROL_ENABLE_LENGTH); + frame.set_data(packed_data); + + return frame; +} + +std::optional SygnalHpoInterface::createControlCommandFrame( + uint8_t message_id, double value, std::string & error_message) +{ + polymath::socketcan::CanFrame frame; + uint8_t buffer[CAN_MAX_DLC]; + hpo_control_control_command_t unpacked; + + if (0 != hpo_control_control_command_init(&unpacked)) { + error_message += "Unable to initialize hpo_control_control_command_t. \n"; + return std::nullopt; + } + + unpacked.bus_address = bus_address_; + unpacked.message_id = message_id; + // Count8 rolling counter is hard-coded to 0 for now (design doc open question #1). + unpacked.count8 = hpo_control_control_command_count8_encode(0.0); + unpacked.value = hpo_control_control_command_value_encode(value); + unpacked.crc = 0; + + if (!hpo_control_control_command_pack(buffer, &unpacked, sizeof(buffer))) { + error_message += "Could not pack hpo_control_control_command_t. \n"; + return std::nullopt; + } + + unpacked.crc = generate_crc8(buffer); + if (!hpo_control_control_command_pack(buffer, &unpacked, sizeof(buffer))) { + error_message += "Could not pack hpo_control_control_command_t. \n"; + return std::nullopt; + } + + std::array packed_data; + for (size_t i = 0; i < packed_data.size(); ++i) { + packed_data[i] = buffer[i]; + } + + frame.set_can_id(HPO_CONTROL_CONTROL_COMMAND_FRAME_ID); + frame.set_len(HPO_CONTROL_CONTROL_COMMAND_LENGTH); + frame.set_data(packed_data); + + return frame; +} + +} // namespace polymath::sygnal diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp new file mode 100644 index 0000000..230e241 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp @@ -0,0 +1,371 @@ +// 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 "sygnal_can_interface_lib/sygnal_hpo_interface.hpp" + +#include +#include +#include +#include + +#if __has_include() + #include +#elif __has_include() + #include +#else + #error "Catch2 headers not found. Please install Catch2 (v2 or v3)." +#endif + +#include "socketcan_adapter/can_frame.hpp" +#include "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_dbc/hpo_control.h" +#include "sygnal_dbc/hpo_error.h" +#include "sygnal_dbc/hpo_heartbeat.h" + +using polymath::sygnal::HPO_NUM_INTERFACES; +using polymath::sygnal::HpoControlResponse; +using polymath::sygnal::HpoErrorStatus; +using polymath::sygnal::SygnalHpoInterface; + +namespace +{ + +constexpr uint8_t TEST_BUS_ADDRESS = 3; +constexpr uint8_t OTHER_BUS_ADDRESS = 4; + +polymath::socketcan::CanFrame makeFrame(uint32_t can_id, const uint8_t * data, uint8_t len) +{ + polymath::socketcan::CanFrame frame; + frame.set_can_id(can_id); + std::array bytes; + bytes.fill(0); + for (size_t i = 0; i < len && i < CAN_MAX_DLC; ++i) { + bytes[i] = data[i]; + } + frame.set_data(bytes); + frame.set_len(len); + return frame; +} + +polymath::socketcan::CanFrame buildHpoHeartbeat( + uint8_t bus_address, const std::array & interface_states, bool overall_state) +{ + hpo_heartbeat_heartbeat_t msg; + hpo_heartbeat_heartbeat_init(&msg); + msg.bus_address = bus_address; + msg.subsystem_id = 0; + msg.system_state = 0; + msg.interface0_state = interface_states[0] ? 1 : 0; + msg.interface1_state = interface_states[1] ? 1 : 0; + msg.interface2_state = interface_states[2] ? 1 : 0; + msg.interface3_state = interface_states[3] ? 1 : 0; + msg.interface4_state = interface_states[4] ? 1 : 0; + msg.interface5_state = interface_states[5] ? 1 : 0; + msg.interface6_state = interface_states[6] ? 1 : 0; + msg.overall_interface_state = overall_state ? 1 : 0; + msg.count16 = 0; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_HEARTBEAT_HEARTBEAT_FRAME_ID, buffer, HPO_HEARTBEAT_HEARTBEAT_LENGTH); +} + +polymath::socketcan::CanFrame buildHpoControlEnableResponse(uint8_t bus_address, uint8_t message_id, bool enable) +{ + hpo_control_control_enable_response_t msg; + hpo_control_control_enable_response_init(&msg); + msg.bus_address = bus_address; + msg.message_id = message_id; + msg.enable = enable ? 1 : 0; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_control_control_enable_response_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_control_control_enable_response_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID, buffer, HPO_CONTROL_CONTROL_ENABLE_RESPONSE_LENGTH); +} + +polymath::socketcan::CanFrame buildHpoControlCommandResponse(uint8_t bus_address, uint8_t message_id, double value) +{ + hpo_control_control_command_response_t msg; + hpo_control_control_command_response_init(&msg); + msg.bus_address = bus_address; + msg.message_id = message_id; + msg.count8 = 0; + msg.value = hpo_control_control_command_response_value_encode(value); + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_control_control_command_response_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_control_control_command_response_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_CONTROL_CONTROL_COMMAND_RESPONSE_FRAME_ID, buffer, HPO_CONTROL_CONTROL_COMMAND_RESPONSE_LENGTH); +} + +polymath::socketcan::CanFrame buildHpoErrorStatus( + uint8_t bus_address, + uint8_t subsystem_id, + uint8_t error_type, + uint16_t error_can_id, + uint8_t section_id, + uint8_t interface_id) +{ + hpo_error_error_status_t msg; + hpo_error_error_status_init(&msg); + msg.bus_address = bus_address; + msg.subsystem_id = subsystem_id; + msg.config_section_id = section_id; + msg.config_interface_id = interface_id; + msg.error_type = error_type; + msg.error_canid = error_can_id; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_error_error_status_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_error_error_status_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_ERROR_ERROR_STATUS_FRAME_ID, buffer, HPO_ERROR_ERROR_STATUS_LENGTH); +} + +} // namespace + +TEST_CASE("SygnalHpoInterface default constructor zeroes interface bits", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo; + REQUIRE(hpo.get_bus_address() == 0); + REQUIRE_FALSE(hpo.get_overall_interface_state()); + for (bool state : hpo.get_interface_states()) { + REQUIRE_FALSE(state); + } +} + +TEST_CASE("SygnalHpoInterface explicit constructor sets bus address", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + REQUIRE(hpo.get_bus_address() == TEST_BUS_ADDRESS); + REQUIRE_FALSE(hpo.get_overall_interface_state()); + for (bool state : hpo.get_interface_states()) { + REQUIRE_FALSE(state); + } +} + +TEST_CASE("SygnalHpoInterface parses heartbeat with all interfaces under HPO control", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::array interfaces{true, true, true, true, true, true, true}; + auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); + + REQUIRE(hpo.parseHeartbeatFrame(frame)); + REQUIRE(hpo.get_overall_interface_state()); + for (bool state : hpo.get_interface_states()) { + REQUIRE(state); + } +} + +TEST_CASE("SygnalHpoInterface parses heartbeat with mixed interface bits", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::array interfaces{true, false, true, false, true, false, true}; + auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, false); + + REQUIRE(hpo.parseHeartbeatFrame(frame)); + REQUIRE_FALSE(hpo.get_overall_interface_state()); + auto states = hpo.get_interface_states(); + for (size_t i = 0; i < HPO_NUM_INTERFACES; ++i) { + REQUIRE(states[i] == interfaces[i]); + } +} + +TEST_CASE("SygnalHpoInterface rejects heartbeat with wrong frame ID", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::array interfaces{true, true, true, true, true, true, true}; + auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); + frame.set_can_id(0x999); + + REQUIRE_FALSE(hpo.parseHeartbeatFrame(frame)); + REQUIRE_FALSE(hpo.get_overall_interface_state()); +} + +TEST_CASE("SygnalHpoInterface rejects heartbeat with bad CRC", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::array interfaces{true, true, true, true, true, true, true}; + auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); + + // Corrupt the CRC byte. + auto bytes = frame.get_data(); + bytes[7] ^= 0xFF; + frame.set_data(bytes); + + REQUIRE_FALSE(hpo.parseHeartbeatFrame(frame)); +} + +TEST_CASE("SygnalHpoInterface rejects heartbeat addressed to a different bus", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::array interfaces{true, true, true, true, true, true, true}; + auto frame = buildHpoHeartbeat(OTHER_BUS_ADDRESS, interfaces, true); + + REQUIRE_FALSE(hpo.parseHeartbeatFrame(frame)); + REQUIRE_FALSE(hpo.get_overall_interface_state()); +} + +TEST_CASE("SygnalHpoInterface creates a ControlEnable frame addressed to itself", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::string error_message; + + auto frame_opt = hpo.createControlEnableFrame(0x42, true, error_message); + REQUIRE(frame_opt.has_value()); + REQUIRE(error_message.empty()); + + auto frame = *frame_opt; + REQUIRE(frame.get_id() == HPO_CONTROL_CONTROL_ENABLE_FRAME_ID); + REQUIRE(frame.get_len() == HPO_CONTROL_CONTROL_ENABLE_LENGTH); + + auto bytes = frame.get_data(); + REQUIRE(polymath::sygnal::check_crc8(reinterpret_cast(bytes.data()))); + + hpo_control_control_enable_t unpacked; + hpo_control_control_enable_init(&unpacked); + REQUIRE( + hpo_control_control_enable_unpack( + &unpacked, reinterpret_cast(bytes.data()), HPO_CONTROL_CONTROL_ENABLE_LENGTH) == 0); + + REQUIRE(unpacked.bus_address == TEST_BUS_ADDRESS); + REQUIRE(unpacked.message_id == 0x42); + REQUIRE(unpacked.enable == 1); +} + +TEST_CASE("SygnalHpoInterface creates a ControlCommand frame addressed to itself", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + std::string error_message; + + auto frame_opt = hpo.createControlCommandFrame(0x07, 0.5, error_message); + REQUIRE(frame_opt.has_value()); + REQUIRE(error_message.empty()); + + auto frame = *frame_opt; + REQUIRE(frame.get_id() == HPO_CONTROL_CONTROL_COMMAND_FRAME_ID); + REQUIRE(frame.get_len() == HPO_CONTROL_CONTROL_COMMAND_LENGTH); + + auto bytes = frame.get_data(); + REQUIRE(polymath::sygnal::check_crc8(reinterpret_cast(bytes.data()))); + + hpo_control_control_command_t unpacked; + hpo_control_control_command_init(&unpacked); + REQUIRE( + hpo_control_control_command_unpack( + &unpacked, reinterpret_cast(bytes.data()), HPO_CONTROL_CONTROL_COMMAND_LENGTH) == 0); + + REQUIRE(unpacked.bus_address == TEST_BUS_ADDRESS); + REQUIRE(unpacked.message_id == 0x07); + REQUIRE(unpacked.count8 == 0); + const double decoded_value = hpo_control_control_command_value_decode(unpacked.value); + REQUIRE(std::fabs(decoded_value - 0.5) < 1e-6); +} + +TEST_CASE("SygnalHpoInterface parses ControlEnableResponse", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoControlEnableResponse(TEST_BUS_ADDRESS, 0x42, true); + + auto response = hpo.parseControlResponse(frame); + REQUIRE(response.has_value()); + REQUIRE(response->is_enable_response); + REQUIRE(response->bus_address == TEST_BUS_ADDRESS); + REQUIRE(response->message_id == 0x42); + REQUIRE(response->enable); +} + +TEST_CASE("SygnalHpoInterface parses ControlCommandResponse", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoControlCommandResponse(TEST_BUS_ADDRESS, 0x07, 0.75); + + auto response = hpo.parseControlResponse(frame); + REQUIRE(response.has_value()); + REQUIRE_FALSE(response->is_enable_response); + REQUIRE(response->bus_address == TEST_BUS_ADDRESS); + REQUIRE(response->message_id == 0x07); + REQUIRE(std::fabs(response->value - 0.75) < 1e-6); +} + +TEST_CASE("SygnalHpoInterface rejects control response for a different bus", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoControlEnableResponse(OTHER_BUS_ADDRESS, 0x42, true); + + REQUIRE_FALSE(hpo.parseControlResponse(frame).has_value()); +} + +TEST_CASE("SygnalHpoInterface rejects control response with unrelated frame ID", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoControlEnableResponse(TEST_BUS_ADDRESS, 0x42, true); + frame.set_can_id(0x999); + + REQUIRE_FALSE(hpo.parseControlResponse(frame).has_value()); +} + +TEST_CASE("SygnalHpoInterface rejects control response with bad CRC", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoControlCommandResponse(TEST_BUS_ADDRESS, 0x07, 0.5); + auto bytes = frame.get_data(); + bytes[7] ^= 0xFF; + frame.set_data(bytes); + + REQUIRE_FALSE(hpo.parseControlResponse(frame).has_value()); +} + +TEST_CASE("SygnalHpoInterface parses ErrorStatus", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoErrorStatus(TEST_BUS_ADDRESS, 1, 5, 0x1234, 6, 2); + + auto err = hpo.parseErrorFrame(frame); + REQUIRE(err.has_value()); + REQUIRE(err->bus_address == TEST_BUS_ADDRESS); + REQUIRE(err->subsystem_id == 1); + REQUIRE(err->error_type == 5); + REQUIRE(err->error_can_id == 0x1234); + REQUIRE(err->config_section_id == 6); + REQUIRE(err->config_interface_id == 2); +} + +TEST_CASE("SygnalHpoInterface rejects error frame for a different bus", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoErrorStatus(OTHER_BUS_ADDRESS, 1, 5, 0x1234, 6, 2); + + REQUIRE_FALSE(hpo.parseErrorFrame(frame).has_value()); +} + +TEST_CASE("SygnalHpoInterface rejects error frame with bad CRC", "[sygnal_hpo_interface]") +{ + SygnalHpoInterface hpo(TEST_BUS_ADDRESS); + auto frame = buildHpoErrorStatus(TEST_BUS_ADDRESS, 1, 5, 0x1234, 6, 2); + auto bytes = frame.get_data(); + bytes[7] ^= 0xFF; + frame.set_data(bytes); + + REQUIRE_FALSE(hpo.parseErrorFrame(frame).has_value()); +} From 50c3df57bbc9e51e862f70ce735031e7629111a9 Mon Sep 17 00:00:00 2001 From: Eric McCarter Date: Wed, 20 May 2026 19:06:17 +0000 Subject: [PATCH 2/2] Add HPO support to SygnalInterfaceSocketcan with CAN-ID arbitration via parse ordering --- .../sygnal_can_interface_lib/CMakeLists.txt | 16 ++ .../sygnal_hpo_interface.hpp | 5 +- .../sygnal_interface_socketcan.hpp | 55 +++- .../src/sygnal_hpo_interface.cpp | 3 +- .../src/sygnal_interface_socketcan.cpp | 165 ++++++++++- .../test/sygnal_hpo_interface_test.cpp | 14 +- ...ygnal_interface_socketcan_routing_test.cpp | 263 ++++++++++++++++++ .../sygnal_can_interface_node.hpp | 1 + .../src/sygnal_can_interface_node.cpp | 16 +- .../src/sygnal_can_interface_params.yaml | 9 + 10 files changed, 531 insertions(+), 16 deletions(-) create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_routing_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt index f7ba09b..d3cbb14 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -125,6 +125,22 @@ if(BUILD_TESTING) WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ) + add_executable(sygnal_interface_socketcan_routing_tests + test/sygnal_interface_socketcan_routing_test.cpp + ) + target_link_libraries(sygnal_interface_socketcan_routing_tests + PRIVATE ${PROJECT_NAME} sygnal_dbc::sygnal_dbc Catch2::Catch2WithMain + ) + ament_add_test( + sygnal_interface_socketcan_routing_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + -r junit -s + -o test_results/${PROJECT_NAME}/sygnal_interface_socketcan_routing_tests_output.xml + ENV CATCH_CONFIG_CONSOLE_WIDTH=120 + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ) + if(DEFINED ENV{CAN_AVAILABLE}) add_executable(sygnal_interface_socketcan_tests test/sygnal_interface_socketcan_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp index 7a58a3e..d41535b 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp @@ -25,7 +25,10 @@ namespace polymath::sygnal { -constexpr uint8_t HPO_NUM_INTERFACES = 7; +// HPO hardware exposes 5 interfaces (0-4). The DBC defines 7 signals (0-6) but the +// remaining two are unused / always zero on real boards; we ignore them, mirroring the +// same DBC-vs-hardware mismatch already handled by the MCM heartbeat parser. +constexpr uint8_t HPO_NUM_INTERFACES = 5; /// @brief Parsed HPO ControlEnableResponse / ControlCommandResponse. /// is_enable_response disambiguates which payload is meaningful: diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp index 448e9d4..340c59d 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp @@ -25,6 +25,7 @@ #include "socketcan_adapter/socketcan_adapter.hpp" #include "sygnal_can_interface_lib/sygnal_command_interface.hpp" +#include "sygnal_can_interface_lib/sygnal_hpo_interface.hpp" #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" namespace polymath::sygnal @@ -44,6 +45,20 @@ struct McmId uint8_t subsystem_id; }; +/// @brief Identifies one HPO endpoint by its CAN bus address. +struct HpoId +{ + uint8_t bus_id; +}; + +/// @brief Result of an HPO send operation. Mirrors SendCommandResult but is typed for HpoControlResponse +/// because the HPO response carries a `message_id` instead of MCM's interface_id/subsystem_id. +struct SendHpoCommandResult +{ + bool success; + std::optional> response_future; +}; + /// @brief Represents a single control interface in Sygnal's System. /// Interfaces can either take floats(default) or ints as inputs. struct InterfaceEndpoint @@ -76,8 +91,15 @@ class SygnalInterfaceSocketcan /// @brief Constructor /// @param socketcan_adapter Shared pointer to socketcan adapter for CAN communication /// @param mcm_ids Flat list of MCM endpoints to manage, each identified by bus and subsystem ID + /// @param hpo_ids Flat list of HPO endpoints to manage, each identified by bus ID. Empty by + /// default; an empty list disables all HPO behavior. The constructor throws + /// std::invalid_argument if any HPO bus_id collides with an MCM bus_id, since + /// disjoint bus addresses are required to route CAN-ID-overloaded frames correctly + /// (see parse() comments). SygnalInterfaceSocketcan( - std::shared_ptr socketcan_adapter, const std::vector & mcm_ids); + std::shared_ptr socketcan_adapter, + const std::vector & mcm_ids, + const std::vector & hpo_ids = {}); /// @brief Parse incoming CAN frame for MCM heartbeat and command responses /// @param frame CAN frame to parse @@ -157,15 +179,46 @@ class SygnalInterfaceSocketcan SendCommandResult sendRelayCommand( InterfaceEndpoint interface, bool relay_state, bool expect_reply, std::string & error_message); + /// @brief Send an HPO ControlEnable command. + /// @param bus_id Bus address of the target HPO. Must match one of the hpo_ids passed at construction. + /// @param message_id HPO MessageID (8-bit identifier of the specific signal/interface on the HPO). + /// @param enable true to grant HPO control of the signal, false to release back to human control. + /// @param expect_reply If true, returns a future for the ControlEnableResponse; fire-and-forget otherwise. + /// @param[out] error_message Populated on failure. + /// @return Result with success flag and optional response future. + SendHpoCommandResult sendHpoControlEnable( + uint8_t bus_id, uint8_t message_id, bool enable, bool expect_reply, std::string & error_message); + + /// @brief Send an HPO ControlCommand with a float value. + /// @param bus_id Bus address of the target HPO. Must match one of the hpo_ids passed at construction. + /// @param message_id HPO MessageID (8-bit identifier of the specific signal/interface on the HPO). + /// @param value Control value (encoded per DBC SIG_VALTYPE float). + /// @param expect_reply If true, returns a future for the ControlCommandResponse; fire-and-forget otherwise. + /// @param[out] error_message Populated on failure. + /// @return Result with success flag and optional response future. + SendHpoCommandResult sendHpoControlCommand( + uint8_t bus_id, uint8_t message_id, double value, bool expect_reply, std::string & error_message); + + /// @brief Read the cached interface bits from the named HPO's latest heartbeat. + /// @return std::nullopt if no HPO with this bus_address has been registered. + std::optional> get_hpo_interface_states(uint8_t bus_address) const; + + /// @brief Read the cached overall interface bit from the named HPO's latest heartbeat. + /// @return std::nullopt if no HPO with this bus_address has been registered. + std::optional get_hpo_overall_interface_state(uint8_t bus_address) const; + private: std::shared_ptr socketcan_adapter_; std::vector mcms_; + std::vector hpos_; SygnalControlInterface control_interface_; // Promise queues for each response type std::queue> enable_response_promises_; std::queue> control_response_promises_; std::queue> relay_response_promises_; + std::queue> hpo_enable_response_promises_; + std::queue> hpo_command_response_promises_; std::mutex promises_mutex_; }; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp index c675244..07286e4 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp @@ -72,8 +72,7 @@ bool SygnalHpoInterface::parseHeartbeatFrame(const socketcan::CanFrame & frame) hpo_interface_states_[2] = (0 != unpacked.interface2_state); hpo_interface_states_[3] = (0 != unpacked.interface3_state); hpo_interface_states_[4] = (0 != unpacked.interface4_state); - hpo_interface_states_[5] = (0 != unpacked.interface5_state); - hpo_interface_states_[6] = (0 != unpacked.interface6_state); + // Signals interface5_state / interface6_state are present in the DBC but unused on real HPO hardware. hpo_overall_interface_state_ = (0 != unpacked.overall_interface_state); return true; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp index 0800668..8e5d11e 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -24,26 +25,89 @@ namespace polymath::sygnal { SygnalInterfaceSocketcan::SygnalInterfaceSocketcan( - std::shared_ptr socketcan_adapter, const std::vector & mcm_ids) + std::shared_ptr socketcan_adapter, + const std::vector & mcm_ids, + const std::vector & hpo_ids) : socketcan_adapter_(socketcan_adapter) , control_interface_() { + // Sygnal overloads CAN IDs across MCM/HPO/IO devices: a single frame ID (e.g. 0x161 ControlCommandResponse) + // carries different byte layouts depending on which device type produced it. The only field with stable + // semantics across layouts is the 7-bit BusAddress. Per-board parsers filter by bus_address to claim only + // their own frames; the MCM control-response parser (SygnalControlInterface) does not filter, so it would + // misinterpret an HPO response if it ever saw one. We sidestep this by (a) running HPO parsers first in + // parse() so HPO frames never reach the MCM parser, and (b) requiring disjoint bus addresses so the + // ordering can correctly route by elimination. The second invariant is enforced here. + for (const auto & hpo : hpo_ids) { + for (const auto & mcm : mcm_ids) { + if (hpo.bus_id == mcm.bus_id) { + throw std::invalid_argument( + "Bus address " + std::to_string(hpo.bus_id) + + " is assigned to both an MCM and an HPO; bus addresses must be disjoint across device types " + "until Sygnal CAN-ID overlap is resolved structurally."); + } + } + } + mcms_.reserve(mcm_ids.size()); for (const auto & id : mcm_ids) { mcms_.emplace_back(id.bus_id, id.subsystem_id); } + + hpos_.reserve(hpo_ids.size()); + for (const auto & id : hpo_ids) { + hpos_.emplace_back(id.bus_id); + } } bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) { - // Try parsing as MCM heartbeat (each interface checks its own bus/subsystem_id) + // DO NOT REORDER without re-reading the rationale in the constructor comment: + // HPO parsers MUST run before SygnalControlInterface::parseCommandResponseFrame because the latter does + // not filter by bus_address, and HPO control responses share CAN IDs (0x61 / 0x161) with MCM responses. + // Heartbeats and HPO error frames already filter by bus_address inside each per-board parser, so the + // order between MCM heartbeats and HPO heartbeats is arbitrary, but we keep the HPO block contiguous + // for clarity. + + // --- HPO first --- + for (auto & hpo : hpos_) { + if (hpo.parseHeartbeatFrame(frame)) { + return true; + } + } + + for (auto & hpo : hpos_) { + auto hpo_response = hpo.parseControlResponse(frame); + if (!hpo_response.has_value()) { + continue; + } + std::lock_guard lock(promises_mutex_); + auto & queue = hpo_response->is_enable_response ? hpo_enable_response_promises_ : hpo_command_response_promises_; + if (!queue.empty()) { + auto promise = std::move(queue.front()); + queue.pop(); + promise.set_value(*hpo_response); + } + return true; + } + + for (auto & hpo : hpos_) { + // Error frames are parsed (and CRC-checked) but not yet surfaced to callers; see design doc open + // question #2. Claim the frame so the MCM error path (if/when added) doesn't double-handle it. + if (hpo.parseErrorFrame(frame).has_value()) { + return true; + } + } + + // --- MCM --- for (auto & mcm : mcms_) { if (mcm.parseMcmHeartbeatFrame(frame)) { return true; } } - // Try parsing as command response + // Try parsing as a (legacy unified) MCM command response. Safe to run last: every HPO-addressed frame + // has already been claimed above, so anything reaching this line is either an MCM frame or unrelated. auto response = control_interface_.parseCommandResponseFrame(frame); if (!response.has_value()) { return false; @@ -229,4 +293,99 @@ SendCommandResult SygnalInterfaceSocketcan::sendRelayCommand( return sendRelayCommand(interface.bus_id, interface.subsystem_id, relay_state, expect_reply, error_message); } +SendHpoCommandResult SygnalInterfaceSocketcan::sendHpoControlEnable( + uint8_t bus_id, uint8_t message_id, bool enable, bool expect_reply, std::string & error_message) +{ + auto it = std::find_if( + hpos_.begin(), hpos_.end(), [bus_id](const SygnalHpoInterface & h) { return h.get_bus_address() == bus_id; }); + if (it == hpos_.end()) { + error_message += "No HPO registered at bus address " + std::to_string(bus_id) + "\n"; + return {false, std::nullopt}; + } + + auto frame_opt = it->createControlEnableFrame(message_id, enable, error_message); + if (!frame_opt.has_value()) { + return {false, std::nullopt}; + } + + std::optional> future_opt; + if (expect_reply) { + std::promise promise; + future_opt = promise.get_future(); + std::lock_guard lock(promises_mutex_); + if (hpo_enable_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) { + hpo_enable_response_promises_.pop(); + } + hpo_enable_response_promises_.push(std::move(promise)); + } + + auto err = socketcan_adapter_->send(*frame_opt); + if (err.has_value()) { + error_message += "Failed to send HPO control enable: " + err.value() + "\n"; + // The promise was already pushed before the send attempt; surface the future regardless so the caller + // can choose to wait on it (or discard) instead of leaving a dangling future the harness can't observe. + return {false, std::move(future_opt)}; + } + + return {true, std::move(future_opt)}; +} + +SendHpoCommandResult SygnalInterfaceSocketcan::sendHpoControlCommand( + uint8_t bus_id, uint8_t message_id, double value, bool expect_reply, std::string & error_message) +{ + auto it = std::find_if( + hpos_.begin(), hpos_.end(), [bus_id](const SygnalHpoInterface & h) { return h.get_bus_address() == bus_id; }); + if (it == hpos_.end()) { + error_message += "No HPO registered at bus address " + std::to_string(bus_id) + "\n"; + return {false, std::nullopt}; + } + + auto frame_opt = it->createControlCommandFrame(message_id, value, error_message); + if (!frame_opt.has_value()) { + return {false, std::nullopt}; + } + + std::optional> future_opt; + if (expect_reply) { + std::promise promise; + future_opt = promise.get_future(); + std::lock_guard lock(promises_mutex_); + if (hpo_command_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) { + hpo_command_response_promises_.pop(); + } + hpo_command_response_promises_.push(std::move(promise)); + } + + auto err = socketcan_adapter_->send(*frame_opt); + if (err.has_value()) { + error_message += "Failed to send HPO control command: " + err.value() + "\n"; + return {false, std::move(future_opt)}; + } + + return {true, std::move(future_opt)}; +} + +std::optional> SygnalInterfaceSocketcan::get_hpo_interface_states( + uint8_t bus_address) const +{ + auto it = std::find_if(hpos_.begin(), hpos_.end(), [bus_address](const SygnalHpoInterface & h) { + return h.get_bus_address() == bus_address; + }); + if (it == hpos_.end()) { + return std::nullopt; + } + return it->get_interface_states(); +} + +std::optional SygnalInterfaceSocketcan::get_hpo_overall_interface_state(uint8_t bus_address) const +{ + auto it = std::find_if(hpos_.begin(), hpos_.end(), [bus_address](const SygnalHpoInterface & h) { + return h.get_bus_address() == bus_address; + }); + if (it == hpos_.end()) { + return std::nullopt; + } + return it->get_overall_interface_state(); +} + } // namespace polymath::sygnal diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp index 230e241..60d7ab4 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp @@ -71,8 +71,8 @@ polymath::socketcan::CanFrame buildHpoHeartbeat( msg.interface2_state = interface_states[2] ? 1 : 0; msg.interface3_state = interface_states[3] ? 1 : 0; msg.interface4_state = interface_states[4] ? 1 : 0; - msg.interface5_state = interface_states[5] ? 1 : 0; - msg.interface6_state = interface_states[6] ? 1 : 0; + msg.interface5_state = 0; + msg.interface6_state = 0; msg.overall_interface_state = overall_state ? 1 : 0; msg.count16 = 0; msg.crc = 0; @@ -167,7 +167,7 @@ TEST_CASE("SygnalHpoInterface explicit constructor sets bus address", "[sygnal_h TEST_CASE("SygnalHpoInterface parses heartbeat with all interfaces under HPO control", "[sygnal_hpo_interface]") { SygnalHpoInterface hpo(TEST_BUS_ADDRESS); - std::array interfaces{true, true, true, true, true, true, true}; + std::array interfaces{true, true, true, true, true}; auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); REQUIRE(hpo.parseHeartbeatFrame(frame)); @@ -180,7 +180,7 @@ TEST_CASE("SygnalHpoInterface parses heartbeat with all interfaces under HPO con TEST_CASE("SygnalHpoInterface parses heartbeat with mixed interface bits", "[sygnal_hpo_interface]") { SygnalHpoInterface hpo(TEST_BUS_ADDRESS); - std::array interfaces{true, false, true, false, true, false, true}; + std::array interfaces{true, false, true, false, true}; auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, false); REQUIRE(hpo.parseHeartbeatFrame(frame)); @@ -194,7 +194,7 @@ TEST_CASE("SygnalHpoInterface parses heartbeat with mixed interface bits", "[syg TEST_CASE("SygnalHpoInterface rejects heartbeat with wrong frame ID", "[sygnal_hpo_interface]") { SygnalHpoInterface hpo(TEST_BUS_ADDRESS); - std::array interfaces{true, true, true, true, true, true, true}; + std::array interfaces{true, true, true, true, true}; auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); frame.set_can_id(0x999); @@ -205,7 +205,7 @@ TEST_CASE("SygnalHpoInterface rejects heartbeat with wrong frame ID", "[sygnal_h TEST_CASE("SygnalHpoInterface rejects heartbeat with bad CRC", "[sygnal_hpo_interface]") { SygnalHpoInterface hpo(TEST_BUS_ADDRESS); - std::array interfaces{true, true, true, true, true, true, true}; + std::array interfaces{true, true, true, true, true}; auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true); // Corrupt the CRC byte. @@ -219,7 +219,7 @@ TEST_CASE("SygnalHpoInterface rejects heartbeat with bad CRC", "[sygnal_hpo_inte TEST_CASE("SygnalHpoInterface rejects heartbeat addressed to a different bus", "[sygnal_hpo_interface]") { SygnalHpoInterface hpo(TEST_BUS_ADDRESS); - std::array interfaces{true, true, true, true, true, true, true}; + std::array interfaces{true, true, true, true, true}; auto frame = buildHpoHeartbeat(OTHER_BUS_ADDRESS, interfaces, true); REQUIRE_FALSE(hpo.parseHeartbeatFrame(frame)); diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_routing_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_routing_test.cpp new file mode 100644 index 0000000..157df02 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_routing_test.cpp @@ -0,0 +1,263 @@ +// 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. + +// Always-on tests that exercise SygnalInterfaceSocketcan logic that does NOT require a real CAN interface: +// constructor invariants and the parse() routing rules between HPO and MCM. End-to-end vcan tests live in +// sygnal_interface_socketcan_test.cpp. + +#include +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() + #include +#elif __has_include() + #include +#else + #error "Catch2 headers not found. Please install Catch2 (v2 or v3)." +#endif + +#include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" +#include "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" +#include "sygnal_dbc/hpo_control.h" +#include "sygnal_dbc/hpo_heartbeat.h" +#include "sygnal_dbc/mcm_control.h" +#include "sygnal_dbc/mcm_heartbeat.h" + +namespace +{ + +constexpr uint8_t MCM_BUS = 1; +constexpr uint8_t HPO_BUS = 3; + +polymath::socketcan::CanFrame makeFrame(uint32_t can_id, const uint8_t * buffer, uint8_t len) +{ + polymath::socketcan::CanFrame frame; + frame.set_can_id(can_id); + std::array data; + data.fill(0); + for (size_t i = 0; i < len && i < CAN_MAX_DLC; ++i) { + data[i] = buffer[i]; + } + frame.set_data(data); + frame.set_len(len); + return frame; +} + +polymath::socketcan::CanFrame buildHpoHeartbeat(uint8_t bus_address, bool overall_state) +{ + hpo_heartbeat_heartbeat_t msg; + hpo_heartbeat_heartbeat_init(&msg); + msg.bus_address = bus_address; + msg.subsystem_id = 0; + msg.system_state = 0; + msg.interface0_state = 1; + msg.interface1_state = 0; + msg.interface2_state = 1; + msg.interface3_state = 0; + msg.interface4_state = 1; + msg.overall_interface_state = overall_state ? 1 : 0; + msg.count16 = 0; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_HEARTBEAT_HEARTBEAT_FRAME_ID, buffer, HPO_HEARTBEAT_HEARTBEAT_LENGTH); +} + +polymath::socketcan::CanFrame buildHpoControlEnableResponse(uint8_t bus_address, uint8_t message_id, bool enable) +{ + hpo_control_control_enable_response_t msg; + hpo_control_control_enable_response_init(&msg); + msg.bus_address = bus_address; + msg.message_id = message_id; + msg.enable = enable ? 1 : 0; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + hpo_control_control_enable_response_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + hpo_control_control_enable_response_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(HPO_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID, buffer, HPO_CONTROL_CONTROL_ENABLE_RESPONSE_LENGTH); +} + +polymath::socketcan::CanFrame buildMcmHeartbeat(uint8_t bus_address, uint8_t subsystem_id, uint8_t system_state) +{ + mcm_heartbeat_heartbeat_t msg; + mcm_heartbeat_heartbeat_init(&msg); + msg.bus_address = bus_address; + msg.subsystem_id = subsystem_id; + msg.system_state = system_state; + msg.interface0_state = 1; + msg.interface1_state = 1; + msg.interface2_state = 1; + msg.interface3_state = 1; + msg.interface4_state = 1; + msg.interface5_state = 0; + msg.interface6_state = 0; + msg.overall_interface_state = 0; + msg.count16 = 0; + msg.crc = 0; + + uint8_t buffer[CAN_MAX_DLC]; + mcm_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + msg.crc = polymath::sygnal::generate_crc8(buffer); + mcm_heartbeat_heartbeat_pack(buffer, &msg, sizeof(buffer)); + return makeFrame(MCM_HEARTBEAT_HEARTBEAT_FRAME_ID, buffer, MCM_HEARTBEAT_HEARTBEAT_LENGTH); +} + +// SocketcanAdapter constructor stores config only; no socket is opened until openSocket() is called. +// We never open a socket in this file, so these tests work without CAN being available. +std::shared_ptr makeUnopenedAdapter() +{ + return std::make_shared("vcan0"); +} + +} // namespace + +TEST_CASE("Constructor throws when an HPO and MCM share a bus address", "[sygnal_socketcan_routing]") +{ + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{1, 0}, {2, 0}}; + std::vector hpos{{2}}; // collides with second MCM + + REQUIRE_THROWS_AS(polymath::sygnal::SygnalInterfaceSocketcan(adapter, mcms, hpos), std::invalid_argument); +} + +TEST_CASE("Constructor with empty hpo_ids preserves legacy behavior", "[sygnal_socketcan_routing]") +{ + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{1, 0}, {1, 1}}; + + // Two-argument form (legacy) must still compile and not throw. + REQUIRE_NOTHROW(polymath::sygnal::SygnalInterfaceSocketcan(adapter, mcms)); + + // Three-argument form with empty HPO list must also work and behave identically. + REQUIRE_NOTHROW(polymath::sygnal::SygnalInterfaceSocketcan(adapter, mcms, {})); +} + +TEST_CASE("parse() routes HPO heartbeat to HPO state and leaves MCM untouched", "[sygnal_socketcan_routing]") +{ + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{MCM_BUS, 0}}; + std::vector hpos{{HPO_BUS}}; + + polymath::sygnal::SygnalInterfaceSocketcan sygnal(adapter, mcms, hpos); + + auto frame = buildHpoHeartbeat(HPO_BUS, /*overall_state=*/true); + REQUIRE(sygnal.parse(frame)); + + auto hpo_state = sygnal.get_hpo_overall_interface_state(HPO_BUS); + REQUIRE(hpo_state.has_value()); + REQUIRE(hpo_state.value()); + + auto hpo_interfaces = sygnal.get_hpo_interface_states(HPO_BUS); + REQUIRE(hpo_interfaces.has_value()); + REQUIRE((*hpo_interfaces)[0]); + REQUIRE_FALSE((*hpo_interfaces)[1]); + REQUIRE((*hpo_interfaces)[2]); + REQUIRE_FALSE((*hpo_interfaces)[3]); + REQUIRE((*hpo_interfaces)[4]); + + // MCM state untouched: still default FAIL_HARD. + auto mcm_state = sygnal.get_sygnal_mcm_state(MCM_BUS, 0); + REQUIRE(mcm_state.has_value()); + REQUIRE(mcm_state.value() == polymath::sygnal::SygnalSystemState::FAIL_HARD); +} + +TEST_CASE("parse() routes MCM heartbeat to MCM state and leaves HPO untouched", "[sygnal_socketcan_routing]") +{ + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{MCM_BUS, 0}}; + std::vector hpos{{HPO_BUS}}; + + polymath::sygnal::SygnalInterfaceSocketcan sygnal(adapter, mcms, hpos); + + auto frame = buildMcmHeartbeat(MCM_BUS, 0, static_cast(polymath::sygnal::SygnalSystemState::MCM_CONTROL)); + REQUIRE(sygnal.parse(frame)); + + auto mcm_state = sygnal.get_sygnal_mcm_state(MCM_BUS, 0); + REQUIRE(mcm_state.has_value()); + REQUIRE(mcm_state.value() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + + // HPO overall interface state stays at the default false. + auto hpo_state = sygnal.get_hpo_overall_interface_state(HPO_BUS); + REQUIRE(hpo_state.has_value()); + REQUIRE_FALSE(hpo_state.value()); +} + +TEST_CASE( + "parse() routes HPO ControlEnableResponse without disturbing MCM promise queues", "[sygnal_socketcan_routing]") +{ + // This test guards against the central invariant of the tactical mitigation: an HPO control response + // arrives on a CAN ID (0x61) that the MCM control-response parser also accepts. The HPO parser MUST claim + // it first so the MCM-side parser never sees it. We verify by issuing an HPO ControlEnable (which arms the + // HPO enable promise queue), then feeding back an HPO ControlEnableResponse via parse(), and asserting the + // HPO future resolves — implying the MCM-side parser did not consume the frame. + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{MCM_BUS, 0}}; + std::vector hpos{{HPO_BUS}}; + + polymath::sygnal::SygnalInterfaceSocketcan sygnal(adapter, mcms, hpos); + + // Arm the HPO enable promise queue directly by sending an HPO ControlEnable. The actual socketcan_send + // will fail (we never opened the socket), but the promise gets pushed before the send call, so we can + // still observe the future. Wrap in a scope and discard the send result. + std::string err; + std::optional> fut_opt; + { + auto result = + sygnal.sendHpoControlEnable(HPO_BUS, /*message_id=*/0x42, /*enable=*/true, /*expect_reply=*/true, err); + // The send itself fails because no socket is open; that's fine for this routing test — the promise was + // already enqueued before the send attempt, so the future is still valid. + fut_opt = std::move(result.response_future); + } + REQUIRE(fut_opt.has_value()); + + // Now deliver a matching response via parse(). + auto frame = buildHpoControlEnableResponse(HPO_BUS, /*message_id=*/0x42, /*enable=*/true); + REQUIRE(sygnal.parse(frame)); + + // The HPO future must be ready immediately. + REQUIRE(fut_opt->wait_for(std::chrono::seconds(0)) == std::future_status::ready); + auto response = fut_opt->get(); + REQUIRE(response.is_enable_response); + REQUIRE(response.bus_address == HPO_BUS); + REQUIRE(response.message_id == 0x42); + REQUIRE(response.enable); +} + +TEST_CASE("sendHpoControlEnable to an unregistered bus address fails cleanly", "[sygnal_socketcan_routing]") +{ + auto adapter = makeUnopenedAdapter(); + std::vector mcms{{MCM_BUS, 0}}; + std::vector hpos{{HPO_BUS}}; + + polymath::sygnal::SygnalInterfaceSocketcan sygnal(adapter, mcms, hpos); + + std::string err; + auto result = sygnal.sendHpoControlEnable(/*bus_id=*/99, 0x10, true, false, err); + REQUIRE_FALSE(result.success); + REQUIRE_FALSE(result.response_future.has_value()); + REQUIRE_FALSE(err.empty()); +} diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp index 81c12ff..a8a722a 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp +++ b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp @@ -101,6 +101,7 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode sygnal_can_interface_ros2::ParamListener param_listener_; sygnal_can_interface_ros2::Params params_; const std::vector mcm_ids_; + const std::vector hpo_ids_; // SocketCAN and Sygnal components std::shared_ptr socketcan_adapter_; diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp index c190d57..dbf7069 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp @@ -62,6 +62,15 @@ static polymath::sygnal::McmId parse_mcm_id(const std::string & endpoint) return {static_cast(bus), static_cast(sub)}; } +static polymath::sygnal::HpoId parse_hpo_id(int64_t value) +{ + if (0 > value || value > 127) { + throw std::invalid_argument( + "Invalid hpo_endpoints entry '" + std::to_string(value) + "': bus address must be in [0, 127]"); + } + return {static_cast(value)}; +} + } // namespace namespace polymath::sygnal @@ -72,6 +81,7 @@ SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & optio , param_listener_(get_node_parameters_interface()) , params_(param_listener_.get_params()) , mcm_ids_(iife_vector(params_.mcm_endpoints, parse_mcm_id)) +, hpo_ids_(iife_vector(params_.hpo_endpoints, parse_hpo_id)) {} SygnalCanInterfaceNode::~SygnalCanInterfaceNode() @@ -99,8 +109,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - // Initialize Sygnal Interface SocketCAN controller - sygnal_interface_ = std::make_unique(socketcan_adapter_, mcm_ids_); + // Initialize Sygnal Interface SocketCAN controller. Constructor throws if MCM and HPO bus addresses + // overlap; we let that propagate up so on_configure returns FAILURE in the catch block below. + sygnal_interface_ = + std::make_unique(socketcan_adapter_, mcm_ids_, hpo_ids_); // Set up callback to parse incoming messages socketcan_adapter_->setOnReceiveCallback( diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml index 1999ed8..1500d0e 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml @@ -21,3 +21,12 @@ sygnal_can_interface_ros2: default_value: [1-0, 1-1, 2-0, 2-1] description: MCM endpoints, each formatted as '-'. read_only: true + hpo_endpoints: + type: int_array + default_value: [] + description: > + HPO bus addresses to instantiate (0-127). Empty by default, which disables HPO behavior + and matches legacy callers. Each entry must NOT collide with any mcm_endpoints bus_id: + the SygnalInterfaceSocketcan constructor throws on overlap because the tactical CAN-ID + arbitration relies on disjoint bus addresses. + read_only: true