Reconnect implements that missed a TC restart

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<std::uint64_t, std::uint8_t> 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

include/task_controller.hpp

@@ -18,6 +18,7 @@
 #include <map>
 #include <mutex>
 #include <queue>
+#include <vector>
 
 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<std::shared_ptr<isobus::ControlFunction>> 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<isobus::ControlFunction> client, std::uint8_t ddiOffset);
 	void send_section_control_state(std::shared_ptr<isobus::ControlFunction> client, bool enabled);
 	bool is_ddi_settable(std::shared_ptr<isobus::ControlFunction> 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<std::shared_ptr<isobus::ControlFunction>> staleCandidates;
+	std::mutex staleCandidatesMutex;
+	bool staleClientDetectionStarted = false;
 };

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<isobus::ControlFunctionFunctionalities>(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<std::int32_t>(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<int>(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<int>(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<int>(recoveries)
+		          << " of " << static_cast<int>(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

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 <algorithm>
 #include <bitset>
 #include <filesystem>
 #include <fstream>
@@ -410,6 +412,77 @@ MyTCServer::MyTCServer(std::shared_ptr<isobus::InternalControlFunction> internal
 {
 }
 
+MyTCServer::~MyTCServer()
+{
+	if (staleClientDetectionStarted)
+	{
+		isobus::CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(
+		  static_cast<std::uint32_t>(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<std::uint32_t>(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<MyTCServer *>(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<ProcessDataCommands>(message.get_data()[0] & 0x0F);
+	if ((ProcessDataCommands::ClientTask == command) ||
+	    (ProcessDataCommands::Value == command) ||
+	    (ProcessDataCommands::SetValueAndAcknowledge == command))
+	{
+		std::lock_guard<std::mutex> 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<std::shared_ptr<isobus::ControlFunction>> MyTCServer::take_stale_clients()
+{
+	std::vector<std::shared_ptr<isobus::ControlFunction>> candidates;
+	{
+		std::lock_guard<std::mutex> 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<std::shared_ptr<isobus::ControlFunction>> 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<std::mutex> lock(staleCandidatesMutex);
+	staleCandidates.clear();
+}
+
 bool MyTCServer::activate_object_pool(std::shared_ptr<isobus::ControlFunction> partnerCF, ObjectPoolActivationError &activationError, ObjectPoolErrorCodes &objectPoolError, std::uint16_t &parentObjectIDOfFaultyObject, std::uint16_t &faultyObjectID)
 {
 	std::lock_guard<std::recursive_mutex> lock(clientsMutex);