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