diff --git a/include/app.hpp b/include/app.hpp index 6ee3ca7..03a0fb4 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -54,6 +54,12 @@ class Application void send_task_controller_status_message(); + /// @brief Keeps the TC off the bus (no TC Status, no TC server update) for TC_QUIET_PERIOD_MS + /// @param reason Logged, so the log shows why clients are about to reconnect + void start_tc_quiet_period(const std::string &reason); + bool is_tc_quiet(); ///< Also ends an expired quiet period + void recover_stale_clients(); + bool setup_can_hardware(); bool setup_control_functions(); void setup_task_controller_server(); @@ -100,6 +106,16 @@ class Application std::uint8_t nmea2000SequenceIdentifier = 0; std::uint32_t lastJ1939SpeedTransmit = 0; std::uint32_t lastTCStatusTransmit = 0; + + /// Longer than the 6 s a client waits for TC Status before it drops the connection and starts over + static constexpr std::uint32_t TC_QUIET_PERIOD_MS = 6500; + /// Minimum time between two quiet periods started for stale clients, so reconnecting clients get time to finish + static constexpr std::uint32_t STALE_CLIENT_RECOVERY_COOLDOWN_MS = 30000; + /// A client still stale after this many quiet periods does not reconnect this way; stop disturbing the others + static constexpr std::uint8_t MAX_STALE_CLIENT_RECOVERIES = 3; + std::uint32_t tcQuietUntilMs = 0; ///< 0 = not quiet + std::uint32_t lastStaleClientRecoveryMs = 0; + std::map staleClientRecoveries; ///< Quiet periods started per client NAME std::int32_t lastSpeedValue = 0; std::int32_t lastXteValue = 0; std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 839a4fc..b4670ea 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -18,6 +18,7 @@ #include #include #include +#include constexpr std::uint8_t NUMBER_SECTIONS_PER_CONDENSED_MESSAGE = 16; @@ -120,7 +121,25 @@ class MyTCServer : public isobus::TaskControllerServer /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated void send_gnss_quality(std::uint8_t quality); + /// @brief Starts watching for clients that talk to us as if connected although we do not know them. + /// See take_stale_clients(). Call once, after initialize(). + void start_stale_client_detection(); + + /// @brief Returns the clients that sent us a Client Task or Value message since the last call without + /// being one of our active clients. Such a client still believes it is connected (typically because the + /// TC restarted faster than the client's 6 s TC Status timeout) and the stack ignores or NACKs it until + /// it reconnects, which it will not do while it keeps seeing our TC Status. + /// Call from the thread that calls update(). + std::vector> take_stale_clients(); + + /// @brief Drops the stale client candidates collected so far, e.g. ones that arrived while the TC was quiet + void clear_stale_clients(); + + ~MyTCServer() override; + private: + static void on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer); + void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); @@ -146,4 +165,10 @@ class MyTCServer : public isobus::TaskControllerServer /// update_section_states -> send_section_setpoint_states), and get_clients() /// must keep returning a copy, not a reference (see its declaration above). mutable std::recursive_mutex clientsMutex; + + /// @brief Sources of Client Task / Value messages to us, collected on the CAN thread and checked + /// against the active clients in take_stale_clients(). Guarded by staleCandidatesMutex. + std::vector> staleCandidates; + std::mutex staleCandidatesMutex; + bool staleClientDetectionStarted = false; }; diff --git a/src/app.cpp b/src/app.cpp index ed61f32..9dfcc38 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -381,6 +381,10 @@ void Application::setup_task_controller_server() languageInterface.set_language_code(settings->get_language_code()); languageInterface.set_country_code(settings->get_country_code()); tcServer->initialize(); + tcServer->start_stale_client_detection(); + // An implement still connected to a previous run of the TC (a restart takes only a few seconds) would keep + // talking to us without reconnecting, and get NACKed, unless it sees our TC Status stop for longer than 6 s. + start_tc_quiet_period("startup"); tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG tcFunctionalities = std::make_unique(tcCF); tcFunctionalities->set_functionality_is_supported( @@ -626,9 +630,19 @@ bool Application::update() udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); - tcServer->request_measurement_commands(); - warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); - tcServer->update(); + if (is_tc_quiet()) + { + // Not calling update() also holds the stack's own TC Status; messages wait in its queue + lastTcServerUpdateCallMs = 0; + tcServer->clear_stale_clients(); + } + else + { + tcServer->request_measurement_commands(); + warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); + tcServer->update(); + recover_stale_clients(); + } if (tcFunctionalities) tcFunctionalities->update(); if (tecuFunctionalities) @@ -789,7 +803,11 @@ bool Application::update() lastLateWarningMs = isobus::SystemTiming::get_timestamp_ms(); } } - if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) + if (is_tc_quiet()) + { + lastTCStatusTransmit = 0; // Send right away when the quiet period ends, without a "late" warning + } + else if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) { static bool firstStatusSent = false; send_task_controller_status_message(); @@ -933,6 +951,73 @@ Application::ImplementDetails Application::derive_implement_details(ClientState return details; } +void Application::start_tc_quiet_period(const std::string &reason) +{ + tcQuietUntilMs = isobus::SystemTiming::get_timestamp_ms() + TC_QUIET_PERIOD_MS; + if (0 == tcQuietUntilMs) + { + tcQuietUntilMs = 1; // 0 means "not quiet" + } + log("TC") << "Holding TC Status for " << TC_QUIET_PERIOD_MS << " ms (" << reason + << ") so connected implements drop their connection and reconnect" << std::endl; +} + +bool Application::is_tc_quiet() +{ + if (0 == tcQuietUntilMs) + { + return false; + } + // Signed difference so the comparison survives timestamp wrap-around + if (static_cast(isobus::SystemTiming::get_timestamp_ms() - tcQuietUntilMs) < 0) + { + return true; + } + tcQuietUntilMs = 0; + log("TC") << "Quiet period over, sending TC Status again" << std::endl; + return false; +} + +void Application::recover_stale_clients() +{ + for (const auto &client : tcServer->take_stale_clients()) + { + const std::uint64_t name = client->get_NAME().get_full_name(); + std::ostringstream clientText; + clientText << "0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(client->get_address()) + << " (NAME " << std::dec << name << ")"; + + if ((0 != lastStaleClientRecoveryMs) && + !isobus::SystemTiming::time_expired_ms(lastStaleClientRecoveryMs, STALE_CLIENT_RECOVERY_COOLDOWN_MS)) + { + continue; // Give the clients of the last quiet period time to reconnect + } + + auto &recoveries = staleClientRecoveries[name]; + if (recoveries >= MAX_STALE_CLIENT_RECOVERIES) + { + if (MAX_STALE_CLIENT_RECOVERIES == recoveries) + { + log("TC") << "Client " << clientText.str() << " is still talking to us without being connected after " + << static_cast(MAX_STALE_CLIENT_RECOVERIES) + << " quiet periods; giving up. Power-cycle the implement to reconnect it." << std::endl; + send_hardware_message("Implement not reconnecting: power-cycle it", 20, HW_MSG_ALERT); + recoveries++; // Log once + } + continue; + } + + recoveries++; + lastStaleClientRecoveryMs = isobus::SystemTiming::get_timestamp_ms(); + log("TC") << "Client " << clientText.str() << " is talking to us as if connected, but it is not one of our clients " + << "(it missed a TC restart or timeout). Reconnecting it, attempt " << static_cast(recoveries) + << " of " << static_cast(MAX_STALE_CLIENT_RECOVERIES) << "." << std::endl; + send_hardware_message("Reconnecting implement", 5, HW_MSG_INFO); + start_tc_quiet_period("stale client " + clientText.str()); + break; // One quiet period reconnects every client + } +} + void Application::send_task_controller_status_message() { // ISO 11783-10 B.8.1 Task Controller Status message diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 614bb8a..d280f17 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -10,9 +10,11 @@ #include "logging_utils.hpp" #include "settings.hpp" +#include "isobus/isobus/can_network_manager.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" +#include #include #include #include @@ -410,6 +412,77 @@ MyTCServer::MyTCServer(std::shared_ptr internal { } +MyTCServer::~MyTCServer() +{ + if (staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + } +} + +void MyTCServer::start_stale_client_detection() +{ + if (!staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + staleClientDetectionStarted = true; + } +} + +void MyTCServer::on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer) +{ + // Runs on the CAN thread: only collect here, the active client list is checked on the update() thread + auto server = static_cast(parentPointer); + if ((nullptr == server) || + (isobus::CAN_DATA_LENGTH != message.get_data_length()) || + (nullptr == message.get_source_control_function()) || + (server->serverControlFunction != message.get_destination_control_function())) + { + return; + } + + // A client sends these only once it considers itself connected + const auto command = static_cast(message.get_data()[0] & 0x0F); + if ((ProcessDataCommands::ClientTask == command) || + (ProcessDataCommands::Value == command) || + (ProcessDataCommands::SetValueAndAcknowledge == command)) + { + std::lock_guard lock(server->staleCandidatesMutex); + if (server->staleCandidates.end() == std::find(server->staleCandidates.begin(), server->staleCandidates.end(), message.get_source_control_function())) + { + server->staleCandidates.push_back(message.get_source_control_function()); + } + } +} + +std::vector> MyTCServer::take_stale_clients() +{ + std::vector> candidates; + { + std::lock_guard lock(staleCandidatesMutex); + candidates.swap(staleCandidates); + } + + // update() has processed the stack's queue by now, so a client that just connected is already active + std::vector> staleClients; + for (const auto &candidate : candidates) + { + if (candidate->get_address_valid() && (nullptr == get_active_client(candidate))) + { + staleClients.push_back(candidate); + } + } + return staleClients; +} + +void MyTCServer::clear_stale_clients() +{ + std::lock_guard lock(staleCandidatesMutex); + staleCandidates.clear(); +} + bool MyTCServer::activate_object_pool(std::shared_ptr partnerCF, ObjectPoolActivationError &activationError, ObjectPoolErrorCodes &objectPoolError, std::uint16_t &parentObjectIDOfFaultyObject, std::uint16_t &faultyObjectID) { std::lock_guard lock(clientsMutex);