From ec80df3cd26af44addd84f5c64aadb3b6524ae50 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20Gunics?= Date: Sat, 27 Dec 2025 17:08:04 +0100 Subject: [PATCH 1/5] Separate the Tractor ECU function from Task Controller. The ISO Wheel and Radar speed is coming from the Tractor BUS and is provided by the Tractor ECU. The NMEA2000 speed was x10 --- include/app.hpp | 1 + src/app.cpp | 122 ++++++++++++++++++++++++++++++++---------------- 2 files changed, 83 insertions(+), 40 deletions(-) diff --git a/include/app.hpp b/include/app.hpp index f1b782a..784984e 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -35,6 +35,7 @@ class Application std::shared_ptr canDriver; std::shared_ptr tcServer; + std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; std::uint8_t nmea2000SequenceIdentifier = 0; diff --git a/src/app.cpp b/src/app.cpp index ef3b463..aa56814 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -47,27 +47,54 @@ bool Application::initialize() ourNAME.set_arbitrary_address_capable(true); ourNAME.set_industry_group(2); ourNAME.set_device_class(0); - ourNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); ourNAME.set_identity_number(20); ourNAME.set_ecu_instance(0); ourNAME.set_function_instance(0); // TC #1. If you want to change the TC number, change this. ourNAME.set_device_class_instance(0); ourNAME.set_manufacturer_code(1407); - auto serverCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(ourNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 - auto addressClaimedFuture = std::async(std::launch::async, [&serverCF]() { - while (!serverCF->get_address_valid()) - std::this_thread::sleep_for(std::chrono::milliseconds(100)); }); + isobus::NAME tcNAME = ourNAME; + tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); + + isobus::NAME tecuNAME = ourNAME; + tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); + tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed + tecuNAME.set_ecu_instance(0); + + std::cout << "[Init] Creating Task Controller control function..." << std::endl; + auto tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 + auto tcAddressClaimedFuture = std::async(std::launch::async, [&tcCF]() { + while (!tcCF->get_address_valid()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + isobus::CANNetworkManager::CANNetwork.update(); + } + }); // If this fails, probably the update thread is not started - addressClaimedFuture.wait_for(std::chrono::seconds(5)); - if (!serverCF->get_address_valid()) + tcAddressClaimedFuture.wait_for(std::chrono::seconds(5)); + if (!tcCF->get_address_valid()) { std::cout << "Failed to claim address for TC server. The control function might be invalid." << std::endl; return false; } - tcServer = std::make_shared(serverCF); + // Create TECU control function + // TODO: Should we wait between this and TC? + // TODO: If there's already a TECU on the bus we should not create ours + if (tcCF) { // Only create TECU if TC was created + std::cout << "[Init] Creating Tractor ECU control function..." << std::endl; + tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tecuNAME, 0, isobus::preferred_addresses::IndustryGroup2::TractorECU); + std::cout << "[Init] Tractor ECU control function created, waiting 1.5 seconds..." << std::endl; + + // Update the network manager to process TECU CF claiming + for (int i = 0; i < 15; i++) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + isobus::CANNetworkManager::CANNetwork.update(); + } + } + + tcServer = std::make_shared(tcCF); auto &languageInterface = tcServer->get_language_command_interface(); languageInterface.set_language_code("en"); // This is the default, but you can change it if you want languageInterface.set_country_code("US"); // This is the default, but you can change it if you want @@ -75,23 +102,34 @@ bool Application::initialize() tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG // Initialize speed and distance messages - speedMessagesInterface = std::make_unique(serverCF, true, true, true, false); //TODO: make configurable whether to send these messages - speedMessagesInterface->initialize(); - nmea2000MessageInterface = std::make_unique(serverCF, false, false, false, false, false, false, false); - nmea2000MessageInterface->initialize(); - nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages - - speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + if(tecuCF) + { + std::cout << "[Init] Creating Speed Messages Interface on TECU..." << std::endl; + speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages + speedMessagesInterface->initialize(); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + std::cout << "[Init] Speed Messages Interface created and initialized." << std::endl; + + std::cout << "[Init] Creating NMEA2000 Message Interface on TECU..." << std::endl; + nmea2000MessageInterface = std::make_unique(tecuCF, false, false, false, false, false, false, false); + nmea2000MessageInterface->initialize(); + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages + std::cout << "[Init] NMEA2000 Message Interface created and initialized." << std::endl; + } + else + { + std::cout << "[Warning] TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; + } std::cout << "Task controller server started." << std::endl; static std::uint8_t xteSid = 0; static std::uint32_t lastXteTransmit = 0; - auto packetHandler = [this, serverCF](std::uint8_t src, std::uint8_t pgn, std::span data) { + auto packetHandler = [this, tcCF](std::uint8_t src, std::uint8_t pgn, std::span data) { if (src == 0x7F && pgn == 0xFE) // 254 - Steer Data { // TODO: hack to get desired section states. probably want to make a new pgn later when we need more than 16 sections @@ -122,23 +160,26 @@ bool Application::initialize() { std::uint16_t speed = std::abs(value); auto direction = value < 0 ? isobus::SpeedMessagesInterface::MachineDirection::Reverse : isobus::SpeedMessagesInterface::MachineDirection::Forward; - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction); - - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_speed(speed); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_speed(speed); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed); - - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - - auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message(); - cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++); - cog_sog_message.set_speed_over_ground(speed); - cog_sog_message.set_course_over_ground(0); // TODO: Implement course - cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull); + if(speedMessagesInterface) { + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction); + + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_speed(speed); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_speed(speed); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed); + + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + } + if(nmea2000MessageInterface){ + auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message(); + cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++); + cog_sog_message.set_speed_over_ground(speed/10); + cog_sog_message.set_course_over_ground(0); // TODO: Implement course + cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull); + } } else if (identifier == isobus::DataDescriptionIndex::GuidanceLineDeviation) { @@ -160,13 +201,13 @@ bool Application::initialize() }; if (isobus::SystemTiming::time_expired_ms(lastXteTransmit, 1000)) // Transmit every second { - if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), serverCF)) + if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), tcCF)) { lastXteTransmit = isobus::SystemTiming::get_timestamp_ms(); } } } - else if (static_cast(identifier) == 597 /*isobus::DataDescriptionIndex::TotalDistance*/) + else if (static_cast(identifier) == 597 /*isobus::DataDescriptionIndex::TotalDistance*/ && speedMessagesInterface) { auto distance = static_cast(value); speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(distance); @@ -186,14 +227,15 @@ bool Application::initialize() bool Application::update() { static std::uint32_t lastHeartbeatTransmit = 0; + static std::uint32_t lastTECUStatusTransmit = 0; udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); tcServer->request_measurement_commands(); tcServer->update(); - speedMessagesInterface->update(); - nmea2000MessageInterface->update(); + if(speedMessagesInterface) speedMessagesInterface->update(); + if(nmea2000MessageInterface) nmea2000MessageInterface->update(); if (isobus::SystemTiming::time_expired_ms(lastHeartbeatTransmit, 100)) { From 69f9bf46987612294038f2697ca0b0a5f76aaada Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20Gunics?= Date: Sat, 27 Dec 2025 17:14:36 +0100 Subject: [PATCH 2/5] Execute clang-format --- src/app.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/app.cpp b/src/app.cpp index aa56814..ce5fb83 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -82,13 +82,15 @@ bool Application::initialize() // Create TECU control function // TODO: Should we wait between this and TC? // TODO: If there's already a TECU on the bus we should not create ours - if (tcCF) { // Only create TECU if TC was created + if (tcCF) + { // Only create TECU if TC was created std::cout << "[Init] Creating Tractor ECU control function..." << std::endl; tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tecuNAME, 0, isobus::preferred_addresses::IndustryGroup2::TractorECU); std::cout << "[Init] Tractor ECU control function created, waiting 1.5 seconds..." << std::endl; // Update the network manager to process TECU CF claiming - for (int i = 0; i < 15; i++) { + for (int i = 0; i < 15; i++) + { std::this_thread::sleep_for(std::chrono::milliseconds(100)); isobus::CANNetworkManager::CANNetwork.update(); } @@ -102,7 +104,7 @@ bool Application::initialize() tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG // Initialize speed and distance messages - if(tecuCF) + if (tecuCF) { std::cout << "[Init] Creating Speed Messages Interface on TECU..." << std::endl; speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages @@ -119,7 +121,7 @@ bool Application::initialize() nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages std::cout << "[Init] NMEA2000 Message Interface created and initialized." << std::endl; } - else + else { std::cout << "[Warning] TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; } @@ -160,7 +162,8 @@ bool Application::initialize() { std::uint16_t speed = std::abs(value); auto direction = value < 0 ? isobus::SpeedMessagesInterface::MachineDirection::Reverse : isobus::SpeedMessagesInterface::MachineDirection::Forward; - if(speedMessagesInterface) { + if (speedMessagesInterface) + { speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction); speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction); speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction); @@ -173,10 +176,11 @@ bool Application::initialize() speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance } - if(nmea2000MessageInterface){ + if (nmea2000MessageInterface) + { auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message(); cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++); - cog_sog_message.set_speed_over_ground(speed/10); + cog_sog_message.set_speed_over_ground(speed / 10); cog_sog_message.set_course_over_ground(0); // TODO: Implement course cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull); } @@ -234,8 +238,10 @@ bool Application::update() tcServer->request_measurement_commands(); tcServer->update(); - if(speedMessagesInterface) speedMessagesInterface->update(); - if(nmea2000MessageInterface) nmea2000MessageInterface->update(); + if (speedMessagesInterface) + speedMessagesInterface->update(); + if (nmea2000MessageInterface) + nmea2000MessageInterface->update(); if (isobus::SystemTiming::time_expired_ms(lastHeartbeatTransmit, 100)) { From f7104375e73690750d9425145e47f3f3c9b6aa2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20Gunics?= Date: Wed, 31 Dec 2025 00:44:48 +0100 Subject: [PATCH 3/5] Implement J1939 Speed --- include/app.hpp | 2 ++ src/app.cpp | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/include/app.hpp b/include/app.hpp index 784984e..e20086c 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -39,4 +39,6 @@ class Application std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; std::uint8_t nmea2000SequenceIdentifier = 0; + std::uint32_t lastJ1939SpeedTransmit = 0; + std::int32_t lastSpeedValue = 0; }; diff --git a/src/app.cpp b/src/app.cpp index ce5fb83..faa23ba 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -160,6 +160,7 @@ bool Application::initialize() std::int32_t value = data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24); if (identifier == isobus::DataDescriptionIndex::ActualSpeed) { + lastSpeedValue = value; // Store the full precision value std::uint16_t speed = std::abs(value); auto direction = value < 0 ? isobus::SpeedMessagesInterface::MachineDirection::Reverse : isobus::SpeedMessagesInterface::MachineDirection::Forward; if (speedMessagesInterface) @@ -269,6 +270,23 @@ bool Application::update() lastHeartbeatTransmit = isobus::SystemTiming::get_timestamp_ms(); } + // Send J1939 PGN 65256 every 100ms (0.1 seconds) + if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF) + { + std::uint16_t speed_j1939 = (static_cast(std::abs(lastSpeedValue)) * 576u + 312u) / 625u; // mm/s -> (km/h)*256 + std::array j1939_speed_data = { + 0xFF, 0xFF, // Compass Bearing (SPN 165) + static_cast(speed_j1939 & 0xFF), // Machine Speed LSB (byte 2) + static_cast((speed_j1939 >> 8) & 0xFF), // Machine Speed MSB (byte 3) + 0xFF, 0xFF, // Pitch (SPN 583) + 0xFF, 0xFF // Altitude (SPN 580) + }; + if (isobus::CANNetworkManager::CANNetwork.send_can_message(0xFEE8, j1939_speed_data.data(), j1939_speed_data.size(), tecuCF)) + { + lastJ1939SpeedTransmit = isobus::SystemTiming::get_timestamp_ms(); + } + } + return true; } From 084090793c9ffb50b729ccb97c779d1006338eba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20Gunics?= Date: Sat, 27 Dec 2025 17:08:04 +0100 Subject: [PATCH 4/5] Separate the Tractor ECU function from Task Controller. The ISO Wheel and Radar speed is coming from the Tractor BUS and is provided by the Tractor ECU. Implement J1939 Speed The NMEA2000 speed was x10 --- include/app.hpp | 3 + src/app.cpp | 149 +++++++++++++++++++++++++++++++++++------------- 2 files changed, 112 insertions(+), 40 deletions(-) diff --git a/include/app.hpp b/include/app.hpp index f1b782a..e20086c 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -35,7 +35,10 @@ class Application std::shared_ptr canDriver; std::shared_ptr tcServer; + std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; std::uint8_t nmea2000SequenceIdentifier = 0; + std::uint32_t lastJ1939SpeedTransmit = 0; + std::int32_t lastSpeedValue = 0; }; diff --git a/src/app.cpp b/src/app.cpp index ef3b463..187173c 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -47,27 +47,56 @@ bool Application::initialize() ourNAME.set_arbitrary_address_capable(true); ourNAME.set_industry_group(2); ourNAME.set_device_class(0); - ourNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); ourNAME.set_identity_number(20); ourNAME.set_ecu_instance(0); ourNAME.set_function_instance(0); // TC #1. If you want to change the TC number, change this. ourNAME.set_device_class_instance(0); ourNAME.set_manufacturer_code(1407); - auto serverCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(ourNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 - auto addressClaimedFuture = std::async(std::launch::async, [&serverCF]() { - while (!serverCF->get_address_valid()) - std::this_thread::sleep_for(std::chrono::milliseconds(100)); }); + isobus::NAME tcNAME = ourNAME; + tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); + + isobus::NAME tecuNAME = ourNAME; + tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); + tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed + tecuNAME.set_ecu_instance(0); + + std::cout << "[Init] Creating Task Controller control function..." << std::endl; + auto tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 + auto tcAddressClaimedFuture = std::async(std::launch::async, [&tcCF]() { + while (!tcCF->get_address_valid()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + isobus::CANNetworkManager::CANNetwork.update(); + } + }); // If this fails, probably the update thread is not started - addressClaimedFuture.wait_for(std::chrono::seconds(5)); - if (!serverCF->get_address_valid()) + tcAddressClaimedFuture.wait_for(std::chrono::seconds(5)); + if (!tcCF->get_address_valid()) { std::cout << "Failed to claim address for TC server. The control function might be invalid." << std::endl; return false; } - tcServer = std::make_shared(serverCF); + // Create TECU control function + // TODO: Should we wait between this and TC? + // TODO: If there's already a TECU on the bus we should not create ours + if (tcCF) + { // Only create TECU if TC was created + std::cout << "[Init] Creating Tractor ECU control function..." << std::endl; + tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tecuNAME, 0, isobus::preferred_addresses::IndustryGroup2::TractorECU); + std::cout << "[Init] Tractor ECU control function created, waiting 1.5 seconds..." << std::endl; + + // Update the network manager to process TECU CF claiming + for (int i = 0; i < 15; i++) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + isobus::CANNetworkManager::CANNetwork.update(); + } + } + + tcServer = std::make_shared(tcCF); auto &languageInterface = tcServer->get_language_command_interface(); languageInterface.set_language_code("en"); // This is the default, but you can change it if you want languageInterface.set_country_code("US"); // This is the default, but you can change it if you want @@ -75,23 +104,34 @@ bool Application::initialize() tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG // Initialize speed and distance messages - speedMessagesInterface = std::make_unique(serverCF, true, true, true, false); //TODO: make configurable whether to send these messages - speedMessagesInterface->initialize(); - nmea2000MessageInterface = std::make_unique(serverCF, false, false, false, false, false, false, false); - nmea2000MessageInterface->initialize(); - nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages - - speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + if (tecuCF) + { + std::cout << "[Init] Creating Speed Messages Interface on TECU..." << std::endl; + speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages + speedMessagesInterface->initialize(); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + std::cout << "[Init] Speed Messages Interface created and initialized." << std::endl; + + std::cout << "[Init] Creating NMEA2000 Message Interface on TECU..." << std::endl; + nmea2000MessageInterface = std::make_unique(tecuCF, false, false, false, false, false, false, false); + nmea2000MessageInterface->initialize(); + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages + std::cout << "[Init] NMEA2000 Message Interface created and initialized." << std::endl; + } + else + { + std::cout << "[Warning] TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; + } std::cout << "Task controller server started." << std::endl; static std::uint8_t xteSid = 0; static std::uint32_t lastXteTransmit = 0; - auto packetHandler = [this, serverCF](std::uint8_t src, std::uint8_t pgn, std::span data) { + auto packetHandler = [this, tcCF](std::uint8_t src, std::uint8_t pgn, std::span data) { if (src == 0x7F && pgn == 0xFE) // 254 - Steer Data { // TODO: hack to get desired section states. probably want to make a new pgn later when we need more than 16 sections @@ -120,25 +160,31 @@ bool Application::initialize() std::int32_t value = data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24); if (identifier == isobus::DataDescriptionIndex::ActualSpeed) { + lastSpeedValue = value; // Store the full precision value std::uint16_t speed = std::abs(value); auto direction = value < 0 ? isobus::SpeedMessagesInterface::MachineDirection::Reverse : isobus::SpeedMessagesInterface::MachineDirection::Forward; - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction); - - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_speed(speed); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_speed(speed); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed); - - speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance - - auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message(); - cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++); - cog_sog_message.set_speed_over_ground(speed); - cog_sog_message.set_course_over_ground(0); // TODO: Implement course - cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull); + if (speedMessagesInterface) + { + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction); + + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_speed(speed); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_speed(speed); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed); + + speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance + } + if (nmea2000MessageInterface) + { + auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message(); + cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++); + cog_sog_message.set_speed_over_ground(speed / 10); + cog_sog_message.set_course_over_ground(0); // TODO: Implement course + cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull); + } } else if (identifier == isobus::DataDescriptionIndex::GuidanceLineDeviation) { @@ -160,13 +206,13 @@ bool Application::initialize() }; if (isobus::SystemTiming::time_expired_ms(lastXteTransmit, 1000)) // Transmit every second { - if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), serverCF)) + if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), tcCF)) { lastXteTransmit = isobus::SystemTiming::get_timestamp_ms(); } } } - else if (static_cast(identifier) == 597 /*isobus::DataDescriptionIndex::TotalDistance*/) + else if (static_cast(identifier) == 597 /*isobus::DataDescriptionIndex::TotalDistance*/ && speedMessagesInterface) { auto distance = static_cast(value); speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(distance); @@ -186,14 +232,17 @@ bool Application::initialize() bool Application::update() { static std::uint32_t lastHeartbeatTransmit = 0; + static std::uint32_t lastTECUStatusTransmit = 0; udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); tcServer->request_measurement_commands(); tcServer->update(); - speedMessagesInterface->update(); - nmea2000MessageInterface->update(); + if (speedMessagesInterface) + speedMessagesInterface->update(); + if (nmea2000MessageInterface) + nmea2000MessageInterface->update(); if (isobus::SystemTiming::time_expired_ms(lastHeartbeatTransmit, 100)) { @@ -221,6 +270,26 @@ bool Application::update() lastHeartbeatTransmit = isobus::SystemTiming::get_timestamp_ms(); } + // Send J1939 PGN 65256 every 100ms (0.1 seconds) + if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF) + { + std::uint16_t speed_j1939 = (static_cast(std::abs(lastSpeedValue)) * 576u + 312u) / 625u; // mm/s -> (km/h)*256 + std::array j1939_speed_data = { + 0xFF, + 0xFF, // Compass Bearing (SPN 165) + static_cast(speed_j1939 & 0xFF), + static_cast((speed_j1939 >> 8) & 0xFF), // Machine Speed MSB (SPN 517) + 0xFF, + 0xFF, // Pitch (SPN 583) + 0xFF, + 0xFF // Altitude (SPN 580) + }; + if (isobus::CANNetworkManager::CANNetwork.send_can_message(0xFEE8, j1939_speed_data.data(), j1939_speed_data.size(), tecuCF)) + { + lastJ1939SpeedTransmit = isobus::SystemTiming::get_timestamp_ms(); + } + } + return true; } From 09d22e77750bb7e28a6953b701967ff318858d98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20Gunics?= Date: Sat, 3 Jan 2026 21:02:34 +0100 Subject: [PATCH 5/5] Add address_valid calls this should help if we have another TECU on the bus. --- src/app.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/app.cpp b/src/app.cpp index 187173c..a28fb3f 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -104,7 +104,7 @@ bool Application::initialize() tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG // Initialize speed and distance messages - if (tecuCF) + if (tecuCF && tecuCF->get_address_valid()) { std::cout << "[Init] Creating Speed Messages Interface on TECU..." << std::endl; speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages @@ -271,7 +271,7 @@ bool Application::update() } // Send J1939 PGN 65256 every 100ms (0.1 seconds) - if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF) + if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF && tecuCF->get_address_valid()) { std::uint16_t speed_j1939 = (static_cast(std::abs(lastSpeedValue)) * 576u + 312u) / 625u; // mm/s -> (km/h)*256 std::array j1939_speed_data = {