From eb5b8c1a0414cf389d1d91336df470946f7aa14b Mon Sep 17 00:00:00 2001 From: Adrian Del Grosso <10929341+ad3154@users.noreply.github.com> Date: Wed, 21 Feb 2024 18:25:30 -0600 Subject: [PATCH] [TECU]: Add ISOBUS Heartbeat Message Added an interface which manages sending and receiving the ISOBUS heartbeat. Fixed seeder example's manufacturer code. --- examples/seeder_example/seeder.cpp | 2 +- isobus/CMakeLists.txt | 2 + .../can_general_parameter_group_numbers.hpp | 1 + .../isobus/isobus/can_network_manager.hpp | 7 + .../isobus/isobus/isobus_heartbeat.hpp | 154 +++++++++++ isobus/src/can_network_manager.cpp | 30 ++- isobus/src/isobus_heartbeat.cpp | 240 ++++++++++++++++++ test/CMakeLists.txt | 1 + test/diagnostic_protocol_tests.cpp | 2 +- test/heartbeat_tests.cpp | 135 ++++++++++ 10 files changed, 566 insertions(+), 8 deletions(-) create mode 100644 isobus/include/isobus/isobus/isobus_heartbeat.hpp create mode 100644 isobus/src/isobus_heartbeat.cpp create mode 100644 test/heartbeat_tests.cpp diff --git a/examples/seeder_example/seeder.cpp b/examples/seeder_example/seeder.cpp index 68c2b6bc2..65012e69d 100644 --- a/examples/seeder_example/seeder.cpp +++ b/examples/seeder_example/seeder.cpp @@ -64,7 +64,7 @@ bool Seeder::initialize() TestDeviceNAME.set_ecu_instance(0); TestDeviceNAME.set_function_instance(0); TestDeviceNAME.set_device_class_instance(0); - TestDeviceNAME.set_manufacturer_code(64); + TestDeviceNAME.set_manufacturer_code(1407); const isobus::NAMEFilter filterVirtualTerminal(isobus::NAME::NAMEParameters::FunctionCode, static_cast(isobus::NAME::Function::VirtualTerminal)); const isobus::NAMEFilter filterTaskController(isobus::NAME::NAMEParameters::FunctionCode, static_cast(isobus::NAME::Function::TaskController)); diff --git a/isobus/CMakeLists.txt b/isobus/CMakeLists.txt index 9552a7722..2642d6ec7 100644 --- a/isobus/CMakeLists.txt +++ b/isobus/CMakeLists.txt @@ -42,6 +42,7 @@ set(ISOBUS_SRC "isobus_virtual_terminal_objects.cpp" "isobus_virtual_terminal_client_state_tracker.cpp" "isobus_virtual_terminal_client_update_helper.cpp" + "isobus_heartbeat.cpp" "nmea2000_message_definitions.cpp" "nmea2000_message_interface.cpp" "can_message_data.cpp") @@ -88,6 +89,7 @@ set(ISOBUS_INCLUDE "isobus_virtual_terminal_objects.hpp" "isobus_virtual_terminal_client_state_tracker.hpp" "isobus_virtual_terminal_client_update_helper.hpp" + "isobus_heartbeat.hpp" "nmea2000_message_definitions.hpp" "nmea2000_message_interface.hpp" "isobus_preferred_addresses.hpp" diff --git a/isobus/include/isobus/isobus/can_general_parameter_group_numbers.hpp b/isobus/include/isobus/isobus/can_general_parameter_group_numbers.hpp index ebbfb3868..5d97e7dcb 100644 --- a/isobus/include/isobus/isobus/can_general_parameter_group_numbers.hpp +++ b/isobus/include/isobus/isobus/can_general_parameter_group_numbers.hpp @@ -33,6 +33,7 @@ namespace isobus AddressClaim = 0xEE00, ProprietaryA = 0xEF00, MachineSelectedSpeed = 0xF022, + HeartbeatMessage = 0xF0E4, ProductIdentification = 0xFC8D, ControlFunctionFunctionalities = 0xFC8E, DiagnosticProtocolIdentification = 0xFD32, diff --git a/isobus/include/isobus/isobus/can_network_manager.hpp b/isobus/include/isobus/isobus/can_network_manager.hpp index 97ebc267d..d4c29b861 100644 --- a/isobus/include/isobus/isobus/can_network_manager.hpp +++ b/isobus/include/isobus/isobus/can_network_manager.hpp @@ -23,6 +23,7 @@ #include "isobus/isobus/can_message_frame.hpp" #include "isobus/isobus/can_network_configuration.hpp" #include "isobus/isobus/can_transport_protocol.hpp" +#include "isobus/isobus/isobus_heartbeat.hpp" #include "isobus/isobus/nmea2000_fast_packet_protocol.hpp" #include "isobus/utility/event_dispatcher.hpp" #include "isobus/utility/thread_synchronization.hpp" @@ -191,6 +192,11 @@ namespace isobus /// @returns The class instance of the NMEA2k fast packet protocol. std::unique_ptr &get_fast_packet_protocol(std::uint8_t canPortIndex); + /// @brief Returns an interface which can be used to manage ISO11783-7 heartbeat messages. + /// @param[in] canPortIndex The index of the CAN channel associated to the interface you're requesting + /// @returns ISO11783-7 heartbeat interface + HeartbeatInterface &get_heartbeat_interface(std::uint8_t canPortIndex); + /// @brief Returns the configuration of this network manager /// @returns The configuration class for this network manager CANNetworkConfiguration &get_configuration(); @@ -379,6 +385,7 @@ namespace isobus std::array, CAN_PORT_MAXIMUM> transportProtocols; ///< One instance of the transport protocol manager for each channel std::array, CAN_PORT_MAXIMUM> extendedTransportProtocols; ///< One instance of the extended transport protocol manager for each channel std::array, CAN_PORT_MAXIMUM> fastPacketProtocol; ///< One instance of the fast packet protocol for each channel + std::array, CAN_PORT_MAXIMUM> heartBeatInterfaces; ///< Manages ISOBUS heartbeat requests, one per channel std::array, CAN_PORT_MAXIMUM> busloadMessageBitsHistory; ///< Stores the approximate number of bits processed on each channel over multiple previous time windows std::array currentBusloadBitAccumulator; ///< Accumulates the approximate number of bits processed on each channel during the current time window diff --git a/isobus/include/isobus/isobus/isobus_heartbeat.hpp b/isobus/include/isobus/isobus/isobus_heartbeat.hpp new file mode 100644 index 000000000..351d5317c --- /dev/null +++ b/isobus/include/isobus/isobus/isobus_heartbeat.hpp @@ -0,0 +1,154 @@ +//================================================================================================ +/// @file isobus_heartbeat.hpp +/// +/// @brief Defines an interface for sending and receiving ISOBUS heartbeats. +/// The heartbeat message is used to determine the integrity of the communication of messages and +/// parameters being transmitted by a control function. There may be multiple instances of the +/// heartbeat message on the network, and CFs are required transmit the message on request. +/// As long as the heartbeat message is transmitted at the regular +/// time interval and the sequence number increases through the valid range, then the +/// heartbeat message indicates that the data source CF is operational and provides +/// correct data in all its messages. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2024 The Open-Agriculture Developers +//================================================================================================ +#ifndef ISOBUS_HEARTBEAT_HPP +#define ISOBUS_HEARTBEAT_HPP + +#include "isobus/isobus/can_callbacks.hpp" +#include "isobus/isobus/can_internal_control_function.hpp" +#include "isobus/isobus/can_message.hpp" +#include "isobus/utility/event_dispatcher.hpp" + +#include + +namespace isobus +{ + /// @brief This class is used to send and receive ISOBUS heartbeats. + class HeartbeatInterface + { + public: + /// @brief This enum is used to define the possible errors that can occur when receiving a heartbeat. + enum class HeartBeatError + { + InvalidSequenceCounter, ///< The sequence counter is not valid + TimedOut ///< The heartbeat message has not been received within the repetition rate + }; + + /// @brief Constructor for a HeartbeatInterface + /// @param[in] sendCANFrameCallback A callback used to send CAN frames + HeartbeatInterface(const CANMessageFrameCallback &sendCANFrameCallback); + + /// @brief This can be used to disable or enable this heartbeat functionality. + /// It's probably best to leave it enabled for most applications, but it's not + /// strictly needed. + /// @note The interface is enabled by default. + /// @param[in] enable Set to true to enable the interface, or false to disable it. + void set_enabled(bool enable); + + /// @brief Returns if the interface is currently enabled or not. + /// @note The interface is enabled by default. + /// @returns true if the interface is enabled, false if the interface is disabled + bool is_enabled() const; + + /// @brief This method can be used to request that another control function on the bus + /// start sending the heartbeat message. This does not mean the request will be honored. + /// In order to know if your request was accepted, you will need to either + /// register for timeout events, register for heartbeat events, or check to see if your + /// destination control function ever responded at some later time using the various methods + /// available to you on this class' public interface. + /// @note CFs may take up to 250ms to begin sending the heartbeat. + /// @param[in] sourceControlFunction The internal control function to use when sending the request + /// @param[in] destinationControlFunction The destination for the request + /// @returns true if the request was transmitted, otherwise false. + bool request_heartbeat(std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction) const; + + /// @brief Called by the internal control function class when a new internal control function is added. + /// This allows us to respond to requests for heartbeats from other control functions. + /// @param[in] newControlFunction The new internal control function + void on_new_internal_control_function(std::shared_ptr newControlFunction); + + /// @brief Called when an internal control function is deleted. Cleans up stale registrations with + /// PGN request protocol. + /// @param[in] destroyedControlFunction The destroyed internal control function + void on_destroyed_internal_control_function(std::shared_ptr destroyedControlFunction); + + /// @brief Returns an event dispatcher which can be used to register for heartbeat errors. + /// Heartbeat errors are generated when a heartbeat message is not received within the + /// repetition rate, or when the sequence counter is not valid. + /// The control function that generated the error is passed as an argument to the event. + /// @returns An event dispatcher for heartbeat errors + EventDispatcher> &get_heartbeat_error_event_dispatcher(); + + /// @brief Returns an event dispatcher which can be used to register for new tracked heartbeat events. + /// An event will be generated when a new control function is added to the list of CFs sending heartbeats. + /// The control function that generated the error is passed as an argument to the event. + /// @returns An event dispatcher for new tracked heartbeat events + EventDispatcher> &get_new_tracked_heartbeat_event_dispatcher(); + + /// @brief Processes a CAN message, called by the network manager. + /// @param[in] message The CAN message being received + void process_rx_message(const CANMessage &message); + + /// @brief Updates the interface. Called by the network manager, + /// so there is no need for you to call it in your application. + void update(); + + private: + /// @brief This enum is used to define special values for the sequence counter. + enum class SequenceCounterSpecialValue : std::uint8_t + { + Initial = 251, ///< The heartbeat sequence number value shall be set to 251 once upon initialization of a CF. + Error = 254, ///< Sequence Number value 254 indicates an error condition. + NotAvailable = 255 ///< This value shall be used when the transmitted CF is in a shutdown status and is gracefully disconnecting from the network. + }; + + static constexpr std::uint32_t SEQUENCE_TIMEOUT_MS = 300; ///< If the repetition rate exceeds 300 ms an error in the communication is detected. + static constexpr std::uint32_t SEQUENCE_INITIAL_RESPONSE_TIMEOUT_MS = 250; ///< When requesting a heartbeat from another device, If no response for the repetition rate has been received after 250 ms, the requester shall assume that the request was not accepted + static constexpr std::uint32_t SEQUENCE_REPETITION_RATE_MS = 100; ///< A consuming CF shall send a Request for Repetition rate for the heart beat message with a repetition rate of 100 ms + + /// @brief This class is used to store information about a tracked heartbeat. + class Heartbeat + { + public: + /// @brief Constructor for a Heartbeat + /// @param[in] sendingControlFunction The control function that is sending the heartbeat + explicit Heartbeat(std::shared_ptr sendingControlFunction); + + /// @brief Transmits a heartbeat message (for internal control functions only). + /// Updates the sequence counter and timestamp if needed. + /// @param[in] parent The parent interface + /// @returns True if the message is sent, otherwise false. + bool send(const HeartbeatInterface &parent); + + std::shared_ptr controlFunction; ///< The CF that is sending the message + std::uint32_t timestamp_ms; ///< The last time the message was sent by the associated control function + std::uint32_t repetitionRate_ms = SEQUENCE_REPETITION_RATE_MS; ///< For internal control functions, this controls how often the heartbeat is sent. This should really stay at the standard 100ms defined in ISO11783-7. + std::uint8_t sequenceCounter = static_cast(SequenceCounterSpecialValue::Initial); ///< The sequence counter used to validate the heartbeat. Counts from 0-250 normally. + }; + + /// @brief Processes a PGN request for a heartbeat. + /// @param[in] parameterGroupNumber The PGN being requested + /// @param[in] requestingControlFunction The control function that is requesting the heartbeat + /// @param[in] targetControlFunction The control function that is being requested to send the heartbeat + /// @param[in] repetitionRate The repetition rate for the heartbeat + /// @param[in] parentPointer A context variable to find the relevant instance of this class + /// @returns True if the request was transmitted, otherwise false. + static bool process_request_for_heartbeat(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + std::shared_ptr targetControlFunction, + std::uint32_t repetitionRate, + void *parentPointer); + + const CANMessageFrameCallback sendCANFrameCallback; ///< A callback for sending a CAN frame + EventDispatcher> heartbeatErrorEventDispatcher; ///< Event dispatcher for heartbeat errors + EventDispatcher> newTrackedHeartbeatEventDispatcher; ///< Event dispatcher for when a heartbeat message from another control function becomes tracked by this interface + std::list trackedHeartbeats; ///< Store tracked heartbeat data, per CF + bool enabled = true; ///< Attribute that specifies if this interface is enabled. When false, the interface does nothing. + }; +} // namespace isobus + +#endif diff --git a/isobus/src/can_network_manager.cpp b/isobus/src/can_network_manager.cpp index 16f3c3019..25ee4c69e 100644 --- a/isobus/src/can_network_manager.cpp +++ b/isobus/src/can_network_manager.cpp @@ -204,6 +204,14 @@ namespace isobus update_new_partners(); process_rx_messages(); + + // Update ISOBUS heartbeats (should be done before process_tx_messages + // to minimize latency in safety critical paths) + for (std::uint32_t i = 0; i < CAN_PORT_MAXIMUM; i++) + { + heartBeatInterfaces.at(i)->update(); + } + process_tx_messages(); update_internal_cfs(); @@ -305,6 +313,7 @@ namespace isobus { if (ControlFunction::Type::Internal == controlFunction->get_type()) { + heartBeatInterfaces.at(controlFunction->canPortIndex)->on_destroyed_internal_control_function(std::static_pointer_cast(controlFunction)); internalControlFunctions.erase(std::remove(internalControlFunctions.begin(), internalControlFunctions.end(), controlFunction), internalControlFunctions.end()); } else if (ControlFunction::Type::Partnered == controlFunction->get_type()) @@ -358,6 +367,7 @@ namespace isobus void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction, CANLibBadge) { on_control_function_created(controlFunction); + heartBeatInterfaces.at(controlFunction->canPortIndex)->on_new_internal_control_function(std::static_pointer_cast(controlFunction)); } void CANNetworkManager::on_control_function_created(std::shared_ptr controlFunction, CANLibBadge) @@ -435,6 +445,12 @@ namespace isobus return fastPacketProtocol[canPortIndex]; } + HeartbeatInterface &CANNetworkManager::get_heartbeat_interface(std::uint8_t canPortIndex) + { + assert(canPortIndex < CAN_PORT_MAXIMUM); // You passed in an out of range index! + return *heartBeatInterfaces.at(canPortIndex); + } + CANNetworkConfiguration &CANNetworkManager::get_configuration() { return configuration; @@ -501,9 +517,10 @@ namespace isobus i); this->protocol_message_callback(message); }; - transportProtocols[i].reset(new TransportProtocolManager(send_frame_callback, receive_message_callback, &configuration)); - extendedTransportProtocols[i].reset(new ExtendedTransportProtocolManager(send_frame_callback, receive_message_callback, &configuration)); - fastPacketProtocol[i].reset(new FastPacketProtocol(send_frame_callback)); + transportProtocols.at(i).reset(new TransportProtocolManager(send_frame_callback, receive_message_callback, &configuration)); + extendedTransportProtocols.at(i).reset(new ExtendedTransportProtocolManager(send_frame_callback, receive_message_callback, &configuration)); + fastPacketProtocol.at(i).reset(new FastPacketProtocol(send_frame_callback)); + heartBeatInterfaces.at(i).reset(new HeartbeatInterface(send_frame_callback)); } } @@ -1029,9 +1046,10 @@ namespace isobus process_can_message_for_address_violations(currentMessage); // Update Special Callbacks, like protocols and non-cf specific ones - transportProtocols[currentMessage.get_can_port_index()]->process_message(currentMessage); - extendedTransportProtocols[currentMessage.get_can_port_index()]->process_message(currentMessage); - fastPacketProtocol[currentMessage.get_can_port_index()]->process_message(currentMessage); + transportProtocols.at(currentMessage.get_can_port_index())->process_message(currentMessage); + extendedTransportProtocols.at(currentMessage.get_can_port_index())->process_message(currentMessage); + fastPacketProtocol.at(currentMessage.get_can_port_index())->process_message(currentMessage); + heartBeatInterfaces.at(currentMessage.get_can_port_index())->process_rx_message(currentMessage); process_protocol_pgn_callbacks(currentMessage); process_any_control_function_pgn_callbacks(currentMessage); diff --git a/isobus/src/isobus_heartbeat.cpp b/isobus/src/isobus_heartbeat.cpp new file mode 100644 index 000000000..7acb3c4a9 --- /dev/null +++ b/isobus/src/isobus_heartbeat.cpp @@ -0,0 +1,240 @@ +//================================================================================================ +/// @file isobus_heartbeat.cpp +/// +/// @brief Implements an interface for sending and receiving ISOBUS heartbeats. +/// The heartbeat message is used to determine the integrity of the communication of messages and +/// parameters being transmitted by a control function. There may be multiple instances of the +/// heartbeat message on the network, and CFs are required transmit the message on request. +/// As long as the heartbeat message is transmitted at the regular +/// time interval and the sequence number increases through the valid range, then the +/// heartbeat message indicates that the data source CF is operational and provides +/// correct data in all its messages +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2024 The Open-Agriculture Developers +//================================================================================================ +#include "isobus/isobus/isobus_heartbeat.hpp" + +#include "isobus/isobus/can_general_parameter_group_numbers.hpp" +#include "isobus/isobus/can_parameter_group_number_request_protocol.hpp" +#include "isobus/isobus/can_stack_logger.hpp" +#include "isobus/utility/system_timing.hpp" + +namespace isobus +{ + HeartbeatInterface::HeartbeatInterface(const CANMessageFrameCallback &sendCANFrameCallback) : + sendCANFrameCallback(sendCANFrameCallback) + { + } + + void HeartbeatInterface::set_enabled(bool enable) + { + if ((!enable) && (enable != enabled)) + { + LOG_DEBUG("[HB]: Disabling ISOBUS heartbeat interface."); + } + enabled = enable; + } + + bool HeartbeatInterface::is_enabled() const + { + return enabled; + } + + bool HeartbeatInterface::request_heartbeat(std::shared_ptr sourceControlFunction, + std::shared_ptr destinationControlFunction) const + { + bool retVal = false; + + if ((nullptr != sourceControlFunction) && + (nullptr != destinationControlFunction) && + enabled) + { + retVal = ParameterGroupNumberRequestProtocol::request_repetition_rate(static_cast(CANLibParameterGroupNumber::HeartbeatMessage), + SEQUENCE_REPETITION_RATE_MS, + sourceControlFunction, + destinationControlFunction); + } + return retVal; + } + + void HeartbeatInterface::on_new_internal_control_function(std::shared_ptr newControlFunction) + { + auto pgnRequestProtocol = newControlFunction->get_pgn_request_protocol().lock(); + + if (nullptr != pgnRequestProtocol) + { + pgnRequestProtocol->register_request_for_repetition_rate_callback(static_cast(CANLibParameterGroupNumber::HeartbeatMessage), process_request_for_heartbeat, this); + } + } + + void HeartbeatInterface::on_destroyed_internal_control_function(std::shared_ptr destroyedControlFunction) + { + auto pgnRequestProtocol = destroyedControlFunction->get_pgn_request_protocol().lock(); + + if (nullptr != pgnRequestProtocol) + { + pgnRequestProtocol->remove_request_for_repetition_rate_callback(static_cast(CANLibParameterGroupNumber::HeartbeatMessage), process_request_for_heartbeat, this); + } + } + + EventDispatcher> &HeartbeatInterface::get_heartbeat_error_event_dispatcher() + { + return heartbeatErrorEventDispatcher; + } + + EventDispatcher> &HeartbeatInterface::get_new_tracked_heartbeat_event_dispatcher() + { + return newTrackedHeartbeatEventDispatcher; + } + + void HeartbeatInterface::update() + { + if (enabled) + { + trackedHeartbeats.erase(std::remove_if(trackedHeartbeats.begin(), trackedHeartbeats.end(), [this](Heartbeat &heartbeat) { + bool retVal = false; + + if (nullptr != heartbeat.controlFunction) + { + if (ControlFunction::Type::Internal == heartbeat.controlFunction->get_type()) + { + if ((SystemTiming::time_expired_ms(heartbeat.timestamp_ms, heartbeat.repetitionRate_ms)) && + heartbeat.send(*this)) + { + heartbeat.sequenceCounter++; + + if (heartbeat.sequenceCounter > 250) + { + heartbeat.sequenceCounter = 0; + } + } + } + else if (SystemTiming::time_expired_ms(heartbeat.timestamp_ms, SEQUENCE_TIMEOUT_MS)) + { + retVal = true; // External heartbeat is timed-out + LOG_ERROR("[HB]: Heartbeat from control function at address 0x%02X timed out.", heartbeat.controlFunction->get_address()); + heartbeatErrorEventDispatcher.call(HeartBeatError::TimedOut, heartbeat.controlFunction); + } + } + else + { + retVal = true; // Invalid state + } + return retVal; + }), + trackedHeartbeats.end()); + } + } + + HeartbeatInterface::Heartbeat::Heartbeat(std::shared_ptr sendingControlFunction) : + controlFunction(sendingControlFunction), + timestamp_ms(SystemTiming::get_timestamp_ms()) + { + } + + bool HeartbeatInterface::Heartbeat::send(const HeartbeatInterface &parent) + { + bool retVal = false; + const std::array buffer = { sequenceCounter }; + + retVal = parent.sendCANFrameCallback(static_cast(CANLibParameterGroupNumber::HeartbeatMessage), + CANDataSpan(buffer.data(), buffer.size()), + CANNetworkManager::CANNetwork.get_internal_control_function(controlFunction), + nullptr, + CANIdentifier::CANPriority::Priority3); + if (retVal) + { + timestamp_ms = SystemTiming::get_timestamp_ms(); // Sent OK + } + return retVal; + } + + void HeartbeatInterface::process_rx_message(const CANMessage &message) + { + if (enabled && + (static_cast(CANLibParameterGroupNumber::HeartbeatMessage) == message.get_identifier().get_parameter_group_number()) && + (nullptr != message.get_source_control_function()) && + (message.get_data_length() >= 1)) + { + auto managedHeartbeat = std::find_if(trackedHeartbeats.begin(), + trackedHeartbeats.end(), + [&message](const Heartbeat &hb) { + return (message.get_source_control_function() == hb.controlFunction); + }); + + if (managedHeartbeat != trackedHeartbeats.end()) + { + managedHeartbeat->timestamp_ms = SystemTiming::get_timestamp_ms(); + + if (message.get_uint8_at(0) == managedHeartbeat->sequenceCounter) + { + LOG_ERROR("[HB]: Duplicate sequence counter received in heartbeat."); + heartbeatErrorEventDispatcher.call(HeartBeatError::InvalidSequenceCounter, message.get_source_control_function()); + } + else if (message.get_uint8_at(0) != ((managedHeartbeat->sequenceCounter + 1) % 250)) + { + LOG_ERROR("[HB]: Invalid sequence counter received in heartbeat."); + heartbeatErrorEventDispatcher.call(HeartBeatError::InvalidSequenceCounter, message.get_source_control_function()); + } + trackedHeartbeats.back().sequenceCounter = message.get_uint8_at(0); + } + else + { + LOG_DEBUG("[HB]: Tracking new heartbeat from control function at address 0x%02X.", message.get_source_control_function()->get_address()); + + if (message.get_uint8_at(0) != static_cast(HeartbeatInterface::SequenceCounterSpecialValue::Initial)) + { + LOG_WARNING("[HB]: Initial heartbeat sequence counter not received from control function at address 0x%02X.", message.get_source_control_function()->get_address()); + } + + trackedHeartbeats.emplace_back(message.get_source_control_function()); + trackedHeartbeats.back().timestamp_ms = SystemTiming::get_timestamp_ms(); + trackedHeartbeats.back().sequenceCounter = message.get_uint8_at(0); + newTrackedHeartbeatEventDispatcher.call(message.get_source_control_function()); + } + } + } + + bool HeartbeatInterface::process_request_for_heartbeat(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + std::shared_ptr targetControlFunction, + std::uint32_t repetitionRate, + void *parentPointer) + { + bool retVal = false; + + if (nullptr != parentPointer) + { + auto interface = static_cast(parentPointer); + + if ((interface->is_enabled()) && + (static_cast(CANLibParameterGroupNumber::HeartbeatMessage) == parameterGroupNumber)) + { + retVal = true; + + if (SEQUENCE_REPETITION_RATE_MS != repetitionRate) + { + LOG_WARNING("[HB]: Control function at address 0x%02X requested the ISOBUS heartbeat at non-compliant interval. Interval should be 100ms.", requestingControlFunction->get_address()); + } + else + { + LOG_DEBUG("[HB]: Control function at address 0x%02X requested the ISOBUS heartbeat from control function at address 0x%02X.", requestingControlFunction->get_address(), targetControlFunction->get_address()); + } + + auto managedHeartbeat = std::find_if(interface->trackedHeartbeats.begin(), + interface->trackedHeartbeats.end(), + [targetControlFunction](const Heartbeat &hb) { + return (targetControlFunction == hb.controlFunction); + }); + + if (managedHeartbeat == interface->trackedHeartbeats.end()) + { + interface->trackedHeartbeats.emplace_back(targetControlFunction); // Heartbeat will be sent on next update + } + } + } + return retVal; + } +} // namespace isobus diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 71cddd9fa..fa0b9f4fe 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -58,6 +58,7 @@ set(TEST_SRC nmea2000_message_tests.cpp isobus_data_dictionary_tests.cpp can_message_tests.cpp + heartbeat_tests.cpp helpers/control_function_helpers.cpp helpers/messaging_helpers.cpp) diff --git a/test/diagnostic_protocol_tests.cpp b/test/diagnostic_protocol_tests.cpp index 2f4c83a38..6ff785cad 100644 --- a/test/diagnostic_protocol_tests.cpp +++ b/test/diagnostic_protocol_tests.cpp @@ -27,7 +27,7 @@ TEST(DIAGNOSTIC_PROTOCOL_TESTS, CreateAndDestroyProtocolObjects) diagnosticProtocol.reset(); EXPECT_EQ(pgnRequestProtocol->get_number_registered_pgn_request_callbacks(), 0); - EXPECT_EQ(pgnRequestProtocol->get_number_registered_request_for_repetition_rate_callbacks(), 0); + EXPECT_EQ(pgnRequestProtocol->get_number_registered_request_for_repetition_rate_callbacks(), 1); // The heartbeat is registered by default pgnRequestProtocol.reset(); diff --git a/test/heartbeat_tests.cpp b/test/heartbeat_tests.cpp new file mode 100644 index 000000000..0e3547d6e --- /dev/null +++ b/test/heartbeat_tests.cpp @@ -0,0 +1,135 @@ +//================================================================================================ +/// @file heartbeat_tests.cpp +/// +/// @brief Unit tests for the ISOBUS Heartbeat Message interface. +/// +/// @author Adrian Del Grosso +/// +/// @copyright 2024 The Open-Agriculture Developers +//================================================================================================ +#include + +#include "helpers/control_function_helpers.hpp" +#include "helpers/messaging_helpers.hpp" +#include "isobus/hardware_integration/can_hardware_interface.hpp" +#include "isobus/hardware_integration/virtual_can_plugin.hpp" +#include "isobus/isobus/can_network_manager.hpp" +#include "isobus/isobus/isobus_heartbeat.hpp" + +using namespace isobus; + +static bool heartbeat_error_callback_called = false; +static bool new_heartbeat_callback_called = false; +static HeartbeatInterface::HeartBeatError error_type = HeartbeatInterface::HeartBeatError::InvalidSequenceCounter; +void error_callback(HeartbeatInterface::HeartBeatError error, std::shared_ptr) +{ + heartbeat_error_callback_called = true; + error_type = error; +} + +void new_callback(std::shared_ptr) +{ + new_heartbeat_callback_called = true; +} + +TEST(HEARTBEAT_TESTS, HeartBeat) +{ + VirtualCANPlugin testPlugin; + testPlugin.open(); + + CANHardwareInterface::set_number_of_can_channels(1); + CANHardwareInterface::assign_can_channel_frame_handler(0, std::make_shared()); + CANHardwareInterface::start(); + + NAME clientNAME(0); + clientNAME.set_industry_group(2); + clientNAME.set_device_class(4); + clientNAME.set_function_code(static_cast(NAME::Function::EnduranceBraking)); + auto internalECU = test_helpers::claim_internal_control_function(0x41, 0); + auto partner = test_helpers::force_claim_partnered_control_function(0xF4, 0); + + // Get the virtual CAN plugin back to a known state + CANMessageFrame testFrame = {}; + while (!testPlugin.get_queue_empty()) + { + testPlugin.read_frame(testFrame); + } + ASSERT_TRUE(testPlugin.get_queue_empty()); + + auto &heartbeatInterface = CANNetworkManager::CANNetwork.get_heartbeat_interface(0); + + // Enabled by default + EXPECT_TRUE(heartbeatInterface.is_enabled()); + + // Register the error callback + heartbeatInterface.get_heartbeat_error_event_dispatcher().add_listener(error_callback); + + // Register the new heartbeat callback + heartbeatInterface.get_new_tracked_heartbeat_event_dispatcher().add_listener(new_callback); + + heartbeatInterface.request_heartbeat(internalECU, partner); + CANNetworkManager::CANNetwork.update(); + + // Check that the heartbeat request was sent + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + EXPECT_EQ(testFrame.identifier, 0x18CCF441); + EXPECT_EQ(testFrame.dataLength, 8); + EXPECT_EQ(testFrame.data[0], static_cast(61668 & 0xFF)); + EXPECT_EQ(testFrame.data[1], static_cast((61668 >> 8) & 0xFF)); + EXPECT_EQ(testFrame.data[2], static_cast((61668 >> 16) & 0xFF)); + EXPECT_EQ(testFrame.data[3], static_cast(100 & 0xFF)); + EXPECT_EQ(testFrame.data[4], static_cast((100 >> 8) & 0xFF)); + EXPECT_EQ(testFrame.data[5], 0xFF); + EXPECT_EQ(testFrame.data[6], 0xFF); + EXPECT_EQ(testFrame.data[7], 0xFF); + + // Send a request for the heartbeat + testFrame.identifier = 0x18CC41F4; + CANNetworkManager::CANNetwork.process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + EXPECT_EQ(testFrame.identifier, 0x0CF0E441); + EXPECT_EQ(testFrame.dataLength, 1); + EXPECT_EQ(testFrame.data[0], 251); + + // Wait for the next one. Sequence should now be 0 + std::this_thread::sleep_for(std::chrono::milliseconds(80)); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + EXPECT_EQ(testFrame.identifier, 0x0CF0E441); + EXPECT_EQ(testFrame.dataLength, 1); + EXPECT_EQ(testFrame.data[0], 0); + + // Supply a heartbeat + EXPECT_FALSE(new_heartbeat_callback_called); + new_heartbeat_callback_called = false; + testFrame.identifier = 0x0CF0E4F4; + testFrame.dataLength = 1; + testFrame.data[0] = 251; + CANNetworkManager::CANNetwork.process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(new_heartbeat_callback_called); + + // Wait to ensure that the heartbeat times out + EXPECT_FALSE(heartbeat_error_callback_called); + std::this_thread::sleep_for(std::chrono::milliseconds(400)); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(heartbeat_error_callback_called); + EXPECT_EQ(error_type, HeartbeatInterface::HeartBeatError::TimedOut); + + // Get the virtual CAN plugin back to a known state + while (!testPlugin.get_queue_empty()) + { + testPlugin.read_frame(testFrame); + } + ASSERT_TRUE(testPlugin.get_queue_empty()); + + // Disable the heartbeat interface + heartbeatInterface.set_enabled(false); + EXPECT_FALSE(heartbeatInterface.is_enabled()); + + // No message should be sent + EXPECT_FALSE(testPlugin.read_frame(testFrame)); + + CANHardwareInterface::stop(); +}