From 04961a140d35844006bee00fd6b6357706aad7bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 08:42:11 +0200 Subject: [PATCH] Keep implements in automatic mode on AOG's current section demand Section setpoints were only sent when AOG's PGN 229 demand changed. Whenever it did not change, an implement in automatic mode ran on its own state: - After switching to automatic, nothing was sent until a section changed. AOG turns its own Auto button off on every field open/close, so after a field change its demand stays all off and the TC sent nothing at all. In the field a spreader in automatic spread full width for 17 and 47 minutes without receiving a single setpoint. - When the implement's master switch went on, it was not told the current setpoint. - When AOG stopped sending PGN 229 (field closed), the last demand stayed in force, even with sections on. Now the TC keeps AOG's latest demand (all off once PGN 229 stops for 2 s), keeps client setpoints current in manual mode too, and sends every section setpoint plus Setpoint Work State right after section control is enabled, after the implement reports automatic, after an implement master work state goes on, and every 2 s while in automatic. Co-Authored-By: Claude Opus 5.5 --- include/task_controller.hpp | 26 +++++- src/app.cpp | 1 + src/task_controller.cpp | 163 +++++++++++++++++++++++++++++------- 3 files changed, 158 insertions(+), 32 deletions(-) diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 839a4fc..ac6a5e1 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -62,6 +62,11 @@ class ClientState // Element work state management these act like master / override for actual sections void set_element_work_state(std::uint16_t elementNumber, bool isWorking); bool try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const; + /// @brief Asks for every section setpoint to be sent on the next MyTCServer::update_section_setpoints() + void request_full_setpoint(); + /// @brief Whether a full setpoint is due, either requested or because the last one is older than intervalMs + bool is_full_setpoint_due(std::uint32_t nowMs, std::uint32_t intervalMs) const; + void mark_full_setpoint_sent(std::uint32_t nowMs); private: isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC @@ -79,6 +84,8 @@ class ClientState bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting bool usesPerElementControl = false; ///< Legacy mode: use per-element setpoint instead of condensed std::uint16_t perElementSetpointDDI = 0; ///< The DDI to use for per-element setpoints (289 or 141), 0 if not applicable + bool fullSetpointRequested = false; ///< Send every section setpoint on the next update_section_setpoints() + std::uint32_t lastFullSetpointMs = 0; ///< When every section setpoint was last sent, 0 = never }; // Create the task controller server object, this will handle all the ISOBUS communication for us @@ -116,18 +123,35 @@ class MyTCServer : public isobus::TaskControllerServer void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + /// @brief Keeps implements in automatic mode on AOG's section demand. Call from the update() thread. + /// @details Sending only on changes left an implement on its own state whenever AOG's demand did not + /// change: after switching to automatic, after the implement's master switch went on, and when AOG + /// sent no demand at all (no field open). This sends every setpoint right after those events and + /// every SECTION_SETPOINT_REFRESH_MS while in automatic, and treats a missing demand as all off. + void update_section_setpoints(); + /// @brief Sends GNSS quality (DDI 514) to every client that declares that DDI. /// @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); private: - void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); + /// @brief Copies AOG's section demand into a client's section setpoints + /// @returns A bit per condensed block (bit 0 = sections 1-16) whose setpoint changed + std::uint16_t apply_section_demand(ClientState &state); + void send_all_section_setpoint_states(std::shared_ptr client); + void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset, bool forceWorkState = false); void send_section_control_state(std::shared_ptr client, bool enabled); bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); std::map, ClientState> clients; std::map, std::queue>> uploadedPools; + static constexpr std::uint32_t SECTION_SETPOINT_REFRESH_MS = 2000; ///< Resend interval while in automatic + static constexpr std::uint32_t SECTION_DEMAND_TIMEOUT_MS = 2000; ///< AOG sends PGN 229 every frame while a field is open + std::vector aogSectionDemand; ///< Last section states from AOG (PGN 229); all off when none is received + std::uint32_t lastSectionDemandMs = 0; ///< When AOG's section demand was last received, 0 = never + bool sectionDemandReceived = false; ///< Whether AOG's section demand is currently arriving + /// @brief Guards clients and uploadedPools. /// /// CONCURRENCY: the isobus/AgIsoStack stack runs its own background thread diff --git a/src/app.cpp b/src/app.cpp index ed61f32..26c3ac8 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -629,6 +629,7 @@ bool Application::update() tcServer->request_measurement_commands(); warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); tcServer->update(); + tcServer->update_section_setpoints(); if (tcFunctionalities) tcFunctionalities->update(); if (tecuFunctionalities) diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 614bb8a..34c4f55 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -12,7 +12,9 @@ #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" +#include "isobus/utility/system_timing.hpp" +#include #include #include #include @@ -199,6 +201,22 @@ std::uint8_t ClientState::get_section_actual_state(std::uint8_t section) const return SectionState::NOT_INSTALLED; } +void ClientState::request_full_setpoint() +{ + fullSetpointRequested = true; +} + +bool ClientState::is_full_setpoint_due(std::uint32_t nowMs, std::uint32_t intervalMs) const +{ + return fullSetpointRequested || (0 == lastFullSetpointMs) || ((nowMs - lastFullSetpointMs) >= intervalMs); +} + +void ClientState::mark_full_setpoint_sent(std::uint32_t nowMs) +{ + fullSetpointRequested = false; + lastFullSetpointMs = (0 == nowMs) ? 1 : nowMs; +} + bool ClientState::is_any_section_setpoint_on() const { for (std::uint8_t state : sectionSetpointStates) @@ -760,14 +778,30 @@ bool MyTCServer::on_value_command(std::shared_ptr partn case static_cast(isobus::DataDescriptionIndex::SectionControlState): { - clients[partner].set_section_control_enabled(processDataValue == 1); + const bool enabled = (processDataValue == 1); + if (enabled && !clients[partner].is_section_control_enabled()) + { + // The implement went to automatic (possibly from its own terminal): give it our setpoint now + clients[partner].request_full_setpoint(); + } + clients[partner].set_section_control_enabled(enabled); } break; case static_cast(isobus::DataDescriptionIndex::ActualWorkState): { + // Master switch on: some implements then turn every section on until told otherwise + bool wasWorking = false; + const bool isWorking = (processDataValue == 1); + if (isWorking && + (!clients[partner].try_get_element_work_state(elementNumber, wasWorking) || !wasWorking) && + clients[partner].is_section_control_enabled()) + { + clients[partner].request_full_setpoint(); + } + // Store the work state per element (used for parent-off checks) - clients[partner].set_element_work_state(elementNumber, processDataValue == 1); + clients[partner].set_element_work_state(elementNumber, isWorking); // For legacy per-element devices only: propagate to section actual states // so the heartbeat (PGN 0xF0) can report them to AOG. @@ -948,14 +982,16 @@ void MyTCServer::request_measurement_commands() void MyTCServer::update_section_states(std::vector §ionStates) { std::lock_guard lock(clientsMutex); + aogSectionDemand = sectionStates; + lastSectionDemandMs = isobus::SystemTiming::get_timestamp_ms(); + if (0 == lastSectionDemandMs) + { + lastSectionDemandMs = 1; // 0 means "never" + } + for (auto &client : clients) { auto &state = client.second; - if (!state.is_section_control_enabled()) - { - // According to standard, the section setpoint states should only be sent when in auto mode - continue; - } // Skip clients that don't have any sections configured (e.g., tractors or other non-implement devices) if (state.get_number_of_sections() == 0) @@ -963,31 +999,22 @@ void MyTCServer::update_section_states(std::vector §ionStates) continue; } - bool requiresUpdate = false; - for (std::uint8_t i = 0; i < state.get_number_of_sections(); i++) + // Keep the setpoints current in manual mode too, so switching to automatic can send them right away + const std::uint16_t changedBlocks = apply_section_demand(state); + + if (!state.is_section_control_enabled()) { - if ((i % NUMBER_SECTIONS_PER_CONDENSED_MESSAGE == 0) && requiresUpdate) - { - // Send the previous 16 sections - std::uint8_t ddiOffset = (i / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE) - 1; - send_section_setpoint_states(client.first, ddiOffset); - requiresUpdate = false; - } + // According to standard, the section setpoint states should only be sent when in auto mode + continue; + } - if (i < sectionStates.size()) + for (std::uint8_t block = 0; block * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE < state.get_number_of_sections(); block++) + { + if (0 != (changedBlocks & (1u << block))) { - if (sectionStates[i] != (state.get_section_setpoint_state(i) == SectionState::ON)) - { - state.set_section_setpoint_state(i, sectionStates[i] ? SectionState::ON : SectionState::OFF); - requiresUpdate = true; - } + send_section_setpoint_states(client.first, block); } } - if (requiresUpdate) - { - std::uint8_t ddiOffset = (state.get_number_of_sections() - 1) / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; - send_section_setpoint_states(client.first, ddiOffset); - } } } @@ -1008,6 +1035,80 @@ void MyTCServer::update_section_control_enabled(bool enabled) { send_section_control_state(client.first, enabled); } + + if (enabled) + { + // Without this the implement runs on its own state until AOG's demand next changes + client.second.request_full_setpoint(); + } + } +} + +void MyTCServer::update_section_setpoints() +{ + std::lock_guard lock(clientsMutex); + const std::uint32_t nowMs = isobus::SystemTiming::get_timestamp_ms(); + + const bool demandReceived = (0 != lastSectionDemandMs) && !isobus::SystemTiming::time_expired_ms(lastSectionDemandMs, SECTION_DEMAND_TIMEOUT_MS); + if (demandReceived != sectionDemandReceived) + { + sectionDemandReceived = demandReceived; + if (demandReceived) + { + log("TC Server") << "Receiving section states from AOG (PGN 229)" << std::endl; + } + else + { + // AOG stops sending PGN 229 when no field is open. The last demand may have had sections on. + log("TC Server") << "No section states from AOG (PGN 229) for " << SECTION_DEMAND_TIMEOUT_MS + << " ms (no field open?): commanding all sections off" << std::endl; + std::fill(aogSectionDemand.begin(), aogSectionDemand.end(), false); + for (auto &client : clients) + { + apply_section_demand(client.second); + client.second.request_full_setpoint(); + } + } + } + + for (auto &client : clients) + { + auto &state = client.second; + if ((0 == state.get_number_of_sections()) || !state.is_section_control_enabled()) + { + continue; + } + + if (state.is_full_setpoint_due(nowMs, SECTION_SETPOINT_REFRESH_MS)) + { + send_all_section_setpoint_states(client.first); + state.mark_full_setpoint_sent(nowMs); + } + } +} + +std::uint16_t MyTCServer::apply_section_demand(ClientState &state) +{ + std::uint16_t changedBlocks = 0; + for (std::uint8_t i = 0; i < state.get_number_of_sections(); i++) + { + const bool on = (i < aogSectionDemand.size()) && aogSectionDemand[i]; + if (on != (state.get_section_setpoint_state(i) == SectionState::ON)) + { + state.set_section_setpoint_state(i, on ? SectionState::ON : SectionState::OFF); + changedBlocks |= static_cast(1u << (i / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE)); + } + } + return changedBlocks; +} + +void MyTCServer::send_all_section_setpoint_states(std::shared_ptr client) +{ + std::lock_guard lock(clientsMutex); + const std::uint8_t numberOfSections = clients[client].get_number_of_sections(); + for (std::uint8_t block = 0; block * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE < numberOfSections; block++) + { + send_section_setpoint_states(client, block, true); } } @@ -1024,7 +1125,7 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality) } } -void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) +void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset, bool forceWorkState) { std::lock_guard lock(clientsMutex); std::uint8_t sectionOffset = ddiOffset * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; @@ -1044,12 +1145,12 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr(isobus::DataDescriptionIndex::SetpointWorkState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState), setpointWorkState ? 1 : 0); clients[client].set_setpoint_work_state(setpointWorkState); } - else if (!clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) + else if (!forceWorkState && !clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) { log("TC Server") << "DDI 289 (SetpointWorkState) not available!" << std::endl; } @@ -1088,7 +1189,7 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr