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 1/2] [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(); +} From 9dba6b4a8f765ce3cda53622f2d3273696ea6e28 Mon Sep 17 00:00:00 2001 From: Adrian Del Grosso <10929341+ad3154@users.noreply.github.com> Date: Thu, 29 Feb 2024 17:59:25 -0700 Subject: [PATCH 2/2] [Docs]: Add basic sphinx API docs for ISO11783-7 interfaces --- .../api/implement messages/guidance.rst | 25 +++++++++++++++++++ .../api/implement messages/heartbeat.rst | 14 +++++++++++ .../source/api/implement messages/index.rst | 17 +++++++++++++ .../api/implement messages/maintain power.rst | 15 +++++++++++ .../api/implement messages/shortcut.rst | 21 ++++++++++++++++ .../api/implement messages/speed distance.rst | 9 +++++++ sphinx/source/api/index.rst | 1 + 7 files changed, 102 insertions(+) create mode 100644 sphinx/source/api/implement messages/guidance.rst create mode 100644 sphinx/source/api/implement messages/heartbeat.rst create mode 100644 sphinx/source/api/implement messages/index.rst create mode 100644 sphinx/source/api/implement messages/maintain power.rst create mode 100644 sphinx/source/api/implement messages/shortcut.rst create mode 100644 sphinx/source/api/implement messages/speed distance.rst diff --git a/sphinx/source/api/implement messages/guidance.rst b/sphinx/source/api/implement messages/guidance.rst new file mode 100644 index 000000000..447b0c925 --- /dev/null +++ b/sphinx/source/api/implement messages/guidance.rst @@ -0,0 +1,25 @@ +.. _API Guidance: + +ISOBUS Guidance API +=================== + +The guidance API is an interface for sending and receiving ISOBUS guidance messages. +These messages are used to steer ISOBUS compliant machines, steering valves, and implements in general. + + +.. warning:: + + Please use extreme care if you try to steer a machine with this interface! + Remember that this library is licensed under The MIT License, and that by obtaining a + copy of this library and of course by attempting to steer a machine with it, you are agreeing + to our license. + +.. note:: + These messages are expected to be deprecated or at least made redundant in favor + of Tractor Implement Management (TIM) at some point by the AEF, though the timeline on that + is not known at the time of writing this, and it's likely that many machines will + continue to support this interface going forward due to its simplicity over TIM. + This project is not affiliated with the AEF, and the AEF has not endorsed this project. + +.. doxygenclass:: isobus::AgriculturalGuidanceInterface + :members: diff --git a/sphinx/source/api/implement messages/heartbeat.rst b/sphinx/source/api/implement messages/heartbeat.rst new file mode 100644 index 000000000..fb14d6133 --- /dev/null +++ b/sphinx/source/api/implement messages/heartbeat.rst @@ -0,0 +1,14 @@ +.. _API Heartbeat: + +ISOBUS Heartbeat API +==================== + +The heartbeat message (PGN 61668/0xF0E4) 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. + +.. note:: + This interface is enabled by default, but can be disabled if you want to stop your heartbeat(s) or don't care about the safety-critical path of the machine. + +.. doxygenclass:: isobus::HeartbeatInterface + :members: diff --git a/sphinx/source/api/implement messages/index.rst b/sphinx/source/api/implement messages/index.rst new file mode 100644 index 000000000..0e2a14e50 --- /dev/null +++ b/sphinx/source/api/implement messages/index.rst @@ -0,0 +1,17 @@ +.. _API ImplementMessages: + +Implement Messages Application Layer +==================================== + +AgIsoStack++ contains a number of interfaces that are meant to simplify the messages defined in ISO11783-7 for communication between a tractor and implement. +These include the following: + +.. toctree:: + :maxdepth: 1 + :titlesonly: + + heartbeat + guidance + maintain power + shortcut + speed distance diff --git a/sphinx/source/api/implement messages/maintain power.rst b/sphinx/source/api/implement messages/maintain power.rst new file mode 100644 index 000000000..5fae93d1d --- /dev/null +++ b/sphinx/source/api/implement messages/maintain power.rst @@ -0,0 +1,15 @@ +.. _API MaintainPower: + +Maintain Power API +================== + +This interface provides a way to manage sending and receiving the "maintain power" message. +This message is sent by any control function connected to the implement bus and requests that the Tractor ECU (TECU) not switch off the power for 2 s after it has received the wheel-based speed and distance message indicating that the ignition has been switched off. +The message also includes the connected implement(s) operating state. +You can choose if the TECU maintains actuator power independently of ECU power as well, as an option. + +.. note:: + If you are using the library for implement section control, you might want to maintain actuator power using this interface to ensure your section valves close when keyed off. + +.. doxygenclass:: isobus::MaintainPowerInterface + :members: diff --git a/sphinx/source/api/implement messages/shortcut.rst b/sphinx/source/api/implement messages/shortcut.rst new file mode 100644 index 000000000..82903e647 --- /dev/null +++ b/sphinx/source/api/implement messages/shortcut.rst @@ -0,0 +1,21 @@ +.. _API ISB: + +ISOBUS Shortcut Button (ISB) API +================================ + +This is an interface for communicating as or from an ISOBUS shortcut button (ISB). +This functionality is defined in AEF Guideline 004 - ISB and at https://www.isobus.net (ISO 11783-7). + +You can choose to either receive this message, send it, or both. An ISB is essentially +a command to all implements to enter a safe state. See the descriptions located at +https://www.isobus.net/isobus/pGNAndSPN/?type=PGN by searching "All implements stop operations switch state", ISO 11783-7, or +https://www.aef-online.org/fileadmin/user_upload/Content/pdfs/AEF_One_Pager.pdf +for more details. + +.. warning:: + If you consume this message, you **MUST** implement an associated alarm in your + VT object pool, along with an icon or other indication on your home screen that your + working set master supports ISB, as required for AEF conformance. + +.. doxygenclass:: isobus::ShortcutButtonInterface + :members: diff --git a/sphinx/source/api/implement messages/speed distance.rst b/sphinx/source/api/implement messages/speed distance.rst new file mode 100644 index 000000000..07b641098 --- /dev/null +++ b/sphinx/source/api/implement messages/speed distance.rst @@ -0,0 +1,9 @@ +.. _API SpeedDistance: + +Speed and Distance API +====================== + +This is a collection of classes for processing and sending ISOBUS speed messages. + +.. doxygenclass:: isobus::SpeedMessagesInterface + :members: diff --git a/sphinx/source/api/index.rst b/sphinx/source/api/index.rst index 4447bb91b..ec7ad10c3 100644 --- a/sphinx/source/api/index.rst +++ b/sphinx/source/api/index.rst @@ -12,6 +12,7 @@ AgIsoStack++ project. network/index virtual terminal/index task controller/index + implement messages/index .. note::