diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 490e3c9..00b7d82 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -39,6 +39,7 @@ class ClientState std::uint8_t get_section_actual_state(std::uint8_t section) const; std::uint16_t get_element_number_for_section(std::uint8_t section) const; void set_element_number_for_section(std::uint8_t section, std::uint16_t elementNumber); + bool try_get_section_for_element(std::uint16_t elementNumber, std::uint8_t §ion) const; bool is_any_section_setpoint_on() const; bool get_setpoint_work_state() const; void set_setpoint_work_state(bool state); @@ -46,6 +47,10 @@ class ClientState void set_actual_work_state(bool state); bool is_section_control_enabled() const; void set_section_control_enabled(bool state); + bool uses_per_element_control() const; + void set_uses_per_element_control(bool state); + std::uint16_t get_per_element_setpoint_ddi() const; + void set_per_element_setpoint_ddi(std::uint16_t ddi); isobus::DeviceDescriptorObjectPool &get_pool(); bool are_measurement_commands_sent() const; void mark_measurement_commands_sent(); @@ -66,10 +71,13 @@ class ClientState std::vector sectionSetpointStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed) std::vector sectionActualStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed) std::vector sectionToElementNumber; // Maps section index to element number for hierarchy checking + std::map elementToSection; ///< Reverse mapping: element number -> section index bool setpointWorkState = false; ///< The overall work state desired (DDI 289) bool actualWorkState = false; ///< The overall work state actual std::map elementWorkStates; ///< Work state per element (element number -> is working) 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 }; // Create the task controller server object, this will handle all the ISOBUS communication for us diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 93e2320..bf0c1f2 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -72,9 +72,21 @@ void ClientState::set_element_number_for_section(std::uint8_t section, std::uint if (section < numberOfSections && section < sectionToElementNumber.size()) { sectionToElementNumber[section] = elementNumber; + elementToSection[elementNumber] = section; } } +bool ClientState::try_get_section_for_element(std::uint16_t elementNumber, std::uint8_t §ion) const +{ + auto it = elementToSection.find(elementNumber); + if (it != elementToSection.end()) + { + section = it->second; + return true; + } + return false; +} + std::uint8_t ClientState::get_number_of_sections() const { return numberOfSections; @@ -93,7 +105,16 @@ std::uint8_t ClientState::get_section_actual_state(std::uint8_t section) const { if (section < numberOfSections) { - // Check if the element or any parent is off + // For legacy per-element devices, sectionActualStates is updated directly + // from DDI 141 in on_value_command, so we can return it without the + // expensive parent-traversal check (which has thread-safety concerns + // when called from the heartbeat on the main thread). + if (usesPerElementControl) + { + return sectionActualStates[section]; + } + + // For modern/old devices using condensed DDIs, check parent hierarchy std::uint16_t elementNumber = get_element_number_for_section(section); if (is_element_or_parent_off(elementNumber)) { @@ -146,6 +167,26 @@ void ClientState::set_section_control_enabled(bool state) isSectionControlEnabled = state; } +bool ClientState::uses_per_element_control() const +{ + return usesPerElementControl; +} + +void ClientState::set_uses_per_element_control(bool state) +{ + usesPerElementControl = state; +} + +std::uint16_t ClientState::get_per_element_setpoint_ddi() const +{ + return perElementSetpointDDI; +} + +void ClientState::set_per_element_setpoint_ddi(std::uint16_t ddi) +{ + perElementSetpointDDI = ddi; +} + isobus::DeviceDescriptorObjectPool &ClientState::get_pool() { return pool; @@ -362,6 +403,9 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p auto implement = isobus::DeviceDescriptorObjectPoolHelper::get_implement_geometry(state.get_pool()); std::uint8_t numberOfSections = 0; + // Build a flat list of section element numbers in the same order as the geometry enumeration + std::vector sectionElementNumbers; + std::cout << "Implement geometry: " << std::endl; std::cout << "Number of booms=" << implement.booms.size() << std::endl; for (const auto &boom : implement.booms) @@ -373,6 +417,7 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p for (const auto §ion : subBoom.sections) { numberOfSections++; + sectionElementNumbers.push_back(section.elementNumber); std::cout << "Section: id=" << static_cast(section.elementNumber) << std::endl; std::cout << "X Offset: " << section.xOffset_mm.get() << std::endl; std::cout << "Y Offset: " << section.yOffset_mm.get() << std::endl; @@ -383,6 +428,7 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p for (const auto §ion : boom.sections) { numberOfSections++; + sectionElementNumbers.push_back(section.elementNumber); std::cout << "Section: id=" << static_cast(section.elementNumber) << std::endl; std::cout << "X Offset: " << section.xOffset_mm.get() << std::endl; std::cout << "Y Offset: " << section.yOffset_mm.get() << std::endl; @@ -391,6 +437,75 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p } } state.set_number_of_sections(numberOfSections); + + // Map each section index to its element number from the geometry + for (std::uint8_t i = 0; i < numberOfSections && i < sectionElementNumbers.size(); i++) + { + state.set_element_number_for_section(i, sectionElementNumbers[i]); + } + + // Scan the DDOP to determine which section control method the device supports + bool hasCondensedSetpoint = false; // Modern: DDI 290+ (paired with DDI 289 for global work state) + bool hasSettableCondensedActual = false; // Old: DDI 161+ settable + bool hasSettableActualWorkState = false; // Oldest: DDI 141 per-element settable + + for (std::uint32_t i = 0; i < state.get_pool().size(); i++) + { + auto object = state.get_pool().get_object_by_index(i); + if (object->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceProcessData) + { + auto processDataObject = std::dynamic_pointer_cast(object); + auto ddi = processDataObject->get_ddi(); + + if (ddi >= static_cast(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) && + ddi <= static_cast(isobus::DataDescriptionIndex::SetpointCondensedWorkState241_256)) + { + hasCondensedSetpoint = true; + } + if (ddi >= static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState1_16) && + ddi <= static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState241_256) && + processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) + { + hasSettableCondensedActual = true; + } + if (ddi == static_cast(isobus::DataDescriptionIndex::ActualWorkState) && + processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) + { + hasSettableActualWorkState = true; + } + } + } + + // Announce and configure the section control method (hierarchy: Modern > Old > Oldest) + if (hasCondensedSetpoint) + { + // Modern: condensed setpoint DDI 290+ (always paired with DDI 289 for global work state) + std::cout << "[" << get_timestamp() << "] [TC Server] Attempting Section Control via: DDI 290 (SetpointCondensedWorkState) + DDI 289 (SetpointWorkState)" + << " for " << static_cast(numberOfSections) << " sections." << std::endl; + } + else if (hasSettableCondensedActual) + { + // Old: settable condensed actual DDI 161+ + std::cout << "[" << get_timestamp() << "] [TC Server] Attempting Section Control via: DDI 161 (ActualCondensedWorkState, settable)" + << " for " << static_cast(numberOfSections) << " sections." << std::endl; + } + else if (hasSettableActualWorkState) + { + // Oldest: per-element settable DDI 141 + state.set_uses_per_element_control(true); + state.set_per_element_setpoint_ddi(static_cast(isobus::DataDescriptionIndex::ActualWorkState)); + std::cout << "[" << get_timestamp() << "] [TC Server] Attempting Section Control via: DDI 141 (ActualWorkState, settable per-element)" + << " for " << static_cast(numberOfSections) << " sections." << std::endl; + for (std::uint8_t i = 0; i < numberOfSections; i++) + { + std::cout << " Section " << static_cast(i) << " -> element " << sectionElementNumbers[i] << std::endl; + } + } + else + { + std::cout << "[" << get_timestamp() << "] [TC Server] WARNING: No supported section control method detected! " + << "Device has no DDI 290, 161 (settable), or 141 (settable)." << std::endl; + } } else { @@ -519,8 +634,16 @@ bool MyTCServer::on_value_command(std::shared_ptr partn case static_cast(isobus::DataDescriptionIndex::ActualWorkState): { - // Store the work state per element rather than globally + // Store the work state per element (used for parent-off checks) clients[partner].set_element_work_state(elementNumber, processDataValue == 1); + + // For legacy per-element devices: propagate to section actual states + // so the heartbeat (PGN 0xF0) can report them to AOG + std::uint8_t sectionIndex; + if (clients[partner].try_get_section_for_element(elementNumber, sectionIndex)) + { + clients[partner].set_section_actual_state(sectionIndex, (processDataValue == 1) ? SectionState::ON : SectionState::OFF); + } } } @@ -742,6 +865,7 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr(ddiTargetLegacy))) { @@ -753,11 +877,46 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr= clients[client].get_number_of_sections()) + { + break; + } + std::uint16_t elementNumber = clients[client].get_element_number_for_section(sectionIndex); + if (elementNumber != 0) + { + std::uint8_t state = clients[client].get_section_setpoint_state(sectionIndex); + send_set_value(client, setpointDDI, elementNumber, (state == SectionState::ON) ? 1 : 0); + } + } + + // Also send global work state on the boom/device element if DDI 289 is available + bool setpointWorkState = clients[client].is_any_section_setpoint_on(); + if (clients[client].get_setpoint_work_state() != setpointWorkState) + { + if (clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) + { + send_set_value(client, + static_cast(isobus::DataDescriptionIndex::SetpointWorkState), + clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState), + setpointWorkState ? 1 : 0); + clients[client].set_setpoint_work_state(setpointWorkState); + } + } + return; // Per-element path complete } else { - std::cout << "[" << get_timestamp() << "] [TC Server] Neither condensed nor controllable-actual work state supported Missing DDI 290 and 141!" << std::endl; + std::cout << "[" << get_timestamp() << "] [TC Server] No supported method to send section setpoint states! " + << "Device has no DDI 290, 161 (settable), or 141 (settable)." << std::endl; } }