feat: Add ISOBUS bus control function enumeration and TC conflict det…

src/app.cpp

@@ -10,6 +10,7 @@
 
 #include "isobus/hardware_integration/available_can_drivers.hpp"
 #include "isobus/hardware_integration/can_hardware_interface.hpp"
+#include "isobus/isobus/can_internal_control_function.hpp"
 #include "isobus/isobus/can_network_manager.hpp"
 #include "isobus/isobus/isobus_preferred_addresses.hpp"
 #include "isobus/isobus/isobus_standard_data_description_indices.hpp"
@@ -20,11 +21,130 @@
 
 #include "logging_utils.hpp"
 
+#include <iomanip>
 #include <iostream>
+#include <span>
 #include <thread>
 
 using boost::asio::ip::udp;
 
+// Enumerate and log all Control Functions on the bus
+static void enumerate_bus_control_functions(const std::string &context)
+{
+	std::cout << "\n";
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] " << context << std::endl;
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] ==================================================" << std::endl;
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] Control Functions on ISOBUS:" << std::endl;
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] --------------------------------------------------" << std::endl;
+
+	// Get all control functions; offline CFs are filtered out below.
+	auto allCFs = isobus::CANNetworkManager::CANNetwork.get_control_functions(false);
+
+	std::uint32_t cfCount = 0;
+
+	for (const auto &cf : allCFs)
+	{
+		if (!cf)
+			continue;
+
+		// Only show control functions with valid addresses (online)
+		if (!cf->get_address_valid())
+			continue;
+
+		cfCount++;
+		isobus::NAME name = cf->get_NAME();
+		bool isInternal = (cf->get_type() == isobus::ControlFunction::Type::Internal);
+
+		std::cout << "[" << get_timestamp() << "] [Bus CFs]   Address: " << std::setw(3) << std::right << static_cast<int>(cf->get_address())
+		          << " | Mfg: " << std::left << std::setw(5) << name.get_manufacturer_code()
+		          << " | Func: " << std::left << std::setw(3) << static_cast<int>(name.get_function_code())
+		          << " | IG: " << static_cast<int>(name.get_industry_group())
+		          << " | Identity: " << std::setw(6) << std::right << name.get_identity_number()
+		          << " | ECU Inst: " << static_cast<int>(name.get_ecu_instance())
+		          << " | Func Inst: " << static_cast<int>(name.get_function_instance())
+		          << (isInternal ? " [INTERNAL]" : "")
+		          << std::endl;
+	}
+
+	if (cfCount == 0)
+	{
+		std::cout << "[" << get_timestamp() << "] [Bus CFs]   (No control functions detected on bus)" << std::endl;
+	}
+
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] ==================================================" << std::endl;
+	std::cout << "[" << get_timestamp() << "] [Bus CFs] Total CFs found: " << cfCount << std::endl;
+	std::cout << "\n";
+}
+
+// Check for TC address conflicts and log warning if we couldn't claim preferred address
+static void check_tc_address_conflict(const std::shared_ptr<isobus::InternalControlFunction> &ourTC)
+{
+	if (!ourTC || !ourTC->get_address_valid())
+		return;
+
+	static constexpr std::uint8_t PREFERRED_TC_ADDRESS = isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer;
+	static std::uint32_t lastWarnTime = 0;
+	static bool conflictDetected = false;
+
+	// Check if we have the preferred address
+	if (ourTC->get_address() == PREFERRED_TC_ADDRESS)
+	{
+		// We have the preferred address, clear conflict state
+		if (conflictDetected)
+		{
+			conflictDetected = false;
+			std::cout << "[" << get_timestamp() << "] [TC Address] Successfully claimed preferred address " << static_cast<int>(PREFERRED_TC_ADDRESS) << std::endl;
+		}
+		return;
+	}
+
+	// We don't have the preferred address - check if another TC has it
+	auto allCFs = isobus::CANNetworkManager::CANNetwork.get_control_functions(false);
+
+	for (const auto &cf : allCFs)
+	{
+		if (!cf || !cf->get_address_valid())
+			continue;
+
+		if (cf->get_address() == PREFERRED_TC_ADDRESS && cf != ourTC)
+		{
+			isobus::NAME otherName = cf->get_NAME();
+
+			// Check if it's actually a TC
+			std::uint8_t funcCode = otherName.get_function_code();
+			if (funcCode == static_cast<std::uint8_t>(isobus::NAME::Function::TaskController))
+			{
+				// Periodic warning every 30 seconds
+				if (isobus::SystemTiming::time_expired_ms(lastWarnTime, 30000))
+				{
+					conflictDetected = true;
+					std::cout << "\n";
+					std::cout << "[" << get_timestamp() << "] [WARN] ==================================================" << std::endl;
+					std::cout << "[" << get_timestamp() << "] [WARN] TC ADDRESS CONFLICT - Another TC at preferred address " << static_cast<int>(PREFERRED_TC_ADDRESS) << std::endl;
+					std::cout << "[" << get_timestamp() << "] [WARN] Conflicting TC: Mfg=" << otherName.get_manufacturer_code()
+					          << ", Func=" << static_cast<int>(funcCode)
+					          << ", Identity=" << otherName.get_identity_number()
+					          << ", ECU Inst=" << static_cast<int>(otherName.get_ecu_instance())
+					          << ", Func Inst=" << static_cast<int>(otherName.get_function_instance()) << std::endl;
+					std::cout << "[" << get_timestamp() << "] [WARN] Our TC using address: " << static_cast<int>(ourTC->get_address()) << std::endl;
+					std::cout << "[" << get_timestamp() << "] [WARN] ==================================================" << std::endl;
+					std::cout << "\n";
+					lastWarnTime = isobus::SystemTiming::get_timestamp_ms();
+				}
+				return; // Only report first conflicting TC found
+			}
+		}
+	}
+
+	// If we get here, we didn't find a conflicting TC at the preferred address
+	// This means we arbitrated to a different address for another reason
+	if (conflictDetected)
+	{
+		conflictDetected = false;
+		std::cout << "[" << get_timestamp() << "] [TC Address] TC address conflict resolved" << std::endl;
+	}
+}
+
 Application::Application(std::shared_ptr<isobus::CANHardwarePlugin> canDriver) :
   canDriver(canDriver)
 {
@@ -49,6 +169,17 @@ bool Application::initialize()
 
 	isobus::CANNetworkManager::CANNetwork.get_configuration().set_number_of_packets_per_cts_message(255);
 
+	// Start CAN network and allow a brief update window to observe bus CFs
+	auto busDiscoveryWindowStart = isobus::SystemTiming::get_timestamp_ms();
+	while (isobus::SystemTiming::get_time_elapsed_ms(busDiscoveryWindowStart) < 100)
+	{
+		isobus::CANNetworkManager::CANNetwork.update();
+		std::this_thread::sleep_for(std::chrono::milliseconds(10));
+	}
+
+	// Enumerate CFs on the bus BEFORE creating our own functions
+	enumerate_bus_control_functions("Before creating internal control functions");
+
 	isobus::NAME ourNAME(0);
 
 	//! Make sure you change these for your device!!!!
@@ -175,6 +306,9 @@ bool Application::initialize()
 		}
 	}
 
+	// Enumerate CFs on the bus AFTER creating our internal functions
+	enumerate_bus_control_functions("After creating internal control functions");
+
 	// Map settings version to TaskControllerVersion enum
 	isobus::TaskControllerServer::TaskControllerVersion tcVersionEnum;
 	switch (settings->get_tc_version())
@@ -382,6 +516,14 @@ bool Application::update()
 	if (nmea2000MessageInterface)
 		nmea2000MessageInterface->update();
 
+	// Check for TC address conflicts every 15 seconds
+	static std::uint32_t lastConflictCheck = 0;
+	if (isobus::SystemTiming::time_expired_ms(lastConflictCheck, 15000))
+	{
+		check_tc_address_conflict(tcCF);
+		lastConflictCheck = isobus::SystemTiming::get_timestamp_ms();
+	}
+
 	// Send section control heartbeat to AOG every 100ms (PGN 0xF0, source 0x80)
 	// When no clients with sections, send 0 sections as heartbeat so AOG knows TC is alive
 	if (isobus::SystemTiming::time_expired_ms(lastHeartbeatTransmit, 100))