diff --git a/src/app.cpp b/src/app.cpp index 124ae6e..e0b7bbd 100644 --- a/src/app.cpp +++ b/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 #include +#include #include 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(cf->get_address()) + << " | Mfg: " << std::left << std::setw(5) << name.get_manufacturer_code() + << " | Func: " << std::left << std::setw(3) << static_cast(name.get_function_code()) + << " | IG: " << static_cast(name.get_industry_group()) + << " | Identity: " << std::setw(6) << std::right << name.get_identity_number() + << " | ECU Inst: " << static_cast(name.get_ecu_instance()) + << " | Func Inst: " << static_cast(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 &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(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(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(PREFERRED_TC_ADDRESS) << std::endl; + std::cout << "[" << get_timestamp() << "] [WARN] Conflicting TC: Mfg=" << otherName.get_manufacturer_code() + << ", Func=" << static_cast(funcCode) + << ", Identity=" << otherName.get_identity_number() + << ", ECU Inst=" << static_cast(otherName.get_ecu_instance()) + << ", Func Inst=" << static_cast(otherName.get_function_instance()) << std::endl; + std::cout << "[" << get_timestamp() << "] [WARN] Our TC using address: " << static_cast(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 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))