Resolve section control targets from linked process data only

CMakeLists.txt

@@ -194,6 +194,24 @@ if(AOG_TC_VALIDATE_IOP)
   add_dependencies(iop_validator test_async_log)
   add_test(NAME async_log COMMAND test_async_log)
 
+  # Section control method detection against every DDOP in
+  # tools/testdata/implements, expected values from its manifest.csv. Made a
+  # dependency of iop_validator so the validation workflow also builds it.
+  add_executable(
+    test_section_control
+    ${CMAKE_CURRENT_LIST_DIR}/tools/test_section_control_method.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/src/section_control_method.cpp)
+  target_compile_features(test_section_control PRIVATE cxx_std_20)
+  set_target_properties(test_section_control PROPERTIES CXX_EXTENSIONS OFF)
+  target_include_directories(test_section_control
+                             PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include)
+  target_link_libraries(test_section_control PRIVATE isobus::Isobus
+                                                     isobus::Utility)
+  add_dependencies(iop_validator test_section_control)
+  add_test(NAME section_control_method
+           COMMAND test_section_control
+                   "${CMAKE_CURRENT_LIST_DIR}/tools/testdata/implements")
+
   # PASS_REGULAR_EXPRESSION makes CTest ignore the exit code, which is the part
   # a CI gate actually reads.
   add_test(NAME malformed_pool_exit_status COMMAND iop_validator

include/section_control_method.hpp

@@ -0,0 +1,57 @@
+/**
+ * @brief Works out how to switch an implement's sections from its DDOP
+ *
+ * Kept free of the TC server and the CAN stack, so tests can run it against stored DDOPs.
+ */
+
+#pragma once
+
+#include "isobus/isobus/isobus_device_descriptor_object_pool.hpp"
+#include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp"
+
+#include <cstdint>
+#include <map>
+#include <vector>
+
+/// The section control methods, in the order the TC prefers them
+enum class SectionControlMethod : std::uint8_t
+{
+	None, ///< The TC cannot switch the sections
+	CondensedSetpoint, ///< DDI 290+ (Setpoint Condensed Work State), actual states on DDI 161+
+	SettableCondensedActual, ///< DDI 161+ (Actual Condensed Work State) written as the setpoint
+	PerElementSetpointWorkState, ///< DDI 289 on each section element, actual state on DDI 141 of the same element
+	PerElementActualWorkState ///< DDI 141 on each section element written as the setpoint
+};
+
+/// @brief Short name of a method, as used in logs and in tools/testdata/implements/manifest.csv
+const char *to_string(SectionControlMethod method);
+
+/// @brief What to write, and to which element, to switch an implement's sections
+struct SectionControlPlan
+{
+	SectionControlMethod method = SectionControlMethod::None;
+
+	/// Condensed methods: element number per condensed DDI (290+ or 161+), one entry per group of 16 sections
+	std::map<std::uint16_t, std::uint16_t> condensedTargets;
+
+	/// Per-element methods: the setpoint DDI (289 or 141) and the element to write it to for each
+	/// section index. isobus::NULL_OBJECT_ID where that section does not link a settable setpoint.
+	std::uint16_t perElementSetpointDDI = 0;
+	std::vector<std::uint16_t> perElementTargets;
+
+	/// Global Setpoint Work State (DDI 289) on an element that is not a section, if the implement has one
+	bool hasSetpointWorkState = false;
+	std::uint16_t setpointWorkStateElement = 0;
+
+	bool uses_per_element_control() const;
+};
+
+/// @brief Flattens the implement geometry into section element numbers, in AOG's section order
+std::vector<std::uint16_t> get_section_element_numbers(const isobus::DeviceDescriptorObjectPoolHelper::Implement &implement);
+
+/// @brief Picks the section control method and its targets.
+/// Only process data that a device element links counts: some implements send every object of their
+/// template and leave the unused ones unreferenced.
+/// @param[in] pool The client's deserialized DDOP
+/// @param[in] sectionElementNumbers The section element numbers in AOG's section order
+SectionControlPlan resolve_section_control(isobus::DeviceDescriptorObjectPool &pool, const std::vector<std::uint16_t> &sectionElementNumbers);

include/task_controller.hpp

@@ -13,6 +13,7 @@
 #include "isobus/isobus/isobus_device_descriptor_object_pool.hpp"
 #include "isobus/isobus/isobus_standard_data_description_indices.hpp"
 #include "isobus/isobus/isobus_task_controller_server.hpp"
+#include "section_control_method.hpp"
 
 #include <cstdint>
 #include <map>
@@ -49,9 +50,8 @@ class ClientState
 	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);
+	const SectionControlPlan &get_section_control_plan() const;
+	void set_section_control_plan(const SectionControlPlan &plan);
 	isobus::DeviceDescriptorObjectPool &get_pool();
 	bool are_measurement_commands_sent() const;
 	void mark_measurement_commands_sent();
@@ -77,8 +77,7 @@ class ClientState
 	bool actualWorkState = false; ///< The overall work state actual
 	std::map<std::uint16_t, bool> 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
+	SectionControlPlan sectionControlPlan; ///< How to switch the sections, resolved from the DDOP at activation
 };
 
 // Create the task controller server object, this will handle all the ISOBUS communication for us
@@ -123,7 +122,6 @@ class MyTCServer : public isobus::TaskControllerServer
 private:
 	void send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset);
 	void send_section_control_state(std::shared_ptr<isobus::ControlFunction> client, bool enabled);
-	bool is_ddi_settable(std::shared_ptr<isobus::ControlFunction> client, std::uint16_t ddi);
 
 	std::map<std::shared_ptr<isobus::ControlFunction>, ClientState> clients;
 	std::map<std::shared_ptr<isobus::ControlFunction>, std::queue<std::vector<std::uint8_t>>> uploadedPools;

src/section_control_method.cpp

@@ -0,0 +1,198 @@
+/**
+ * @brief Works out how to switch an implement's sections from its DDOP
+ */
+#include "section_control_method.hpp"
+
+#include "isobus/isobus/isobus_standard_data_description_indices.hpp"
+
+#include <algorithm>
+
+using isobus::DataDescriptionIndex;
+using isobus::task_controller_object::DeviceElementObject;
+using isobus::task_controller_object::DeviceProcessDataObject;
+using isobus::task_controller_object::ObjectTypes;
+
+namespace
+{
+	constexpr auto SETPOINT_WORK_STATE = static_cast<std::uint16_t>(DataDescriptionIndex::SetpointWorkState);
+	constexpr auto ACTUAL_WORK_STATE = static_cast<std::uint16_t>(DataDescriptionIndex::ActualWorkState);
+	constexpr auto SETPOINT_CONDENSED_FIRST = static_cast<std::uint16_t>(DataDescriptionIndex::SetpointCondensedWorkState1_16);
+	constexpr auto SETPOINT_CONDENSED_LAST = static_cast<std::uint16_t>(DataDescriptionIndex::SetpointCondensedWorkState241_256);
+	constexpr auto ACTUAL_CONDENSED_FIRST = static_cast<std::uint16_t>(DataDescriptionIndex::ActualCondensedWorkState1_16);
+	constexpr auto ACTUAL_CONDENSED_LAST = static_cast<std::uint16_t>(DataDescriptionIndex::ActualCondensedWorkState241_256);
+
+	/// One process data object as linked from one device element. A process data object that several
+	/// elements link appears once per element; one that no element links does not appear at all.
+	struct LinkedProcessData
+	{
+		std::uint16_t ddi;
+		std::uint16_t elementNumber;
+		bool settable;
+	};
+
+	std::vector<LinkedProcessData> get_linked_process_data(isobus::DeviceDescriptorObjectPool &pool)
+	{
+		std::vector<LinkedProcessData> links;
+		for (std::uint32_t i = 0; i < pool.size(); i++)
+		{
+			auto object = pool.get_object_by_index(i);
+			if (!object || object->get_object_type() != ObjectTypes::DeviceElement)
+			{
+				continue;
+			}
+			auto element = std::static_pointer_cast<DeviceElementObject>(object);
+
+			// Read the children by index. Iterating get_child_object_ids() reads past the end of the list:
+			// AgIsoStack's DataSpan scales end() by sizeof(T) twice (see #110).
+			for (std::uint16_t c = 0; c < element->get_number_child_objects(); c++)
+			{
+				auto child = pool.get_object_by_id(element->get_child_object_id(c));
+				if (!child || child->get_object_type() != ObjectTypes::DeviceProcessData)
+				{
+					continue;
+				}
+				auto processData = std::static_pointer_cast<DeviceProcessDataObject>(child);
+				links.push_back({ processData->get_ddi(),
+				                  element->get_element_number(),
+				                  processData->has_property(DeviceProcessDataObject::PropertiesBit::Settable) });
+			}
+		}
+		return links;
+	}
+
+	bool is_in(std::uint16_t value, std::uint16_t first, std::uint16_t last)
+	{
+		return value >= first && value <= last;
+	}
+
+	// The element of each section index that links a settable setpointDDI, NULL_OBJECT_ID where none does.
+	// Returns an empty list when no section links one.
+	std::vector<std::uint16_t> get_per_element_targets(const std::vector<LinkedProcessData> &links,
+	                                                   const std::vector<std::uint16_t> &sectionElementNumbers,
+	                                                   std::uint16_t setpointDDI)
+	{
+		std::vector<std::uint16_t> targets;
+		bool anyTarget = false;
+		for (auto sectionElement : sectionElementNumbers)
+		{
+			bool linked = std::any_of(links.begin(), links.end(), [&](const LinkedProcessData &link) {
+				return link.ddi == setpointDDI && link.settable && link.elementNumber == sectionElement;
+			});
+			targets.push_back(linked ? sectionElement : isobus::NULL_OBJECT_ID);
+			anyTarget = anyTarget || linked;
+		}
+		return anyTarget ? targets : std::vector<std::uint16_t>();
+	}
+}
+
+const char *to_string(SectionControlMethod method)
+{
+	switch (method)
+	{
+		case SectionControlMethod::CondensedSetpoint:
+			return "DDI290";
+		case SectionControlMethod::SettableCondensedActual:
+			return "DDI161";
+		case SectionControlMethod::PerElementSetpointWorkState:
+			return "DDI289";
+		case SectionControlMethod::PerElementActualWorkState:
+			return "DDI141";
+		default:
+			return "none";
+	}
+}
+
+bool SectionControlPlan::uses_per_element_control() const
+{
+	return method == SectionControlMethod::PerElementSetpointWorkState || method == SectionControlMethod::PerElementActualWorkState;
+}
+
+std::vector<std::uint16_t> get_section_element_numbers(const isobus::DeviceDescriptorObjectPoolHelper::Implement &implement)
+{
+	// Same order as the geometry rows in MyTCServer::activate_object_pool(): the index is AOG's section index
+	std::vector<std::uint16_t> elements;
+	for (const auto &boom : implement.booms)
+	{
+		for (const auto &subBoom : boom.subBooms)
+		{
+			for (const auto &section : subBoom.sections)
+			{
+				elements.push_back(section.elementNumber);
+			}
+		}
+		for (const auto &section : boom.sections)
+		{
+			elements.push_back(section.elementNumber);
+		}
+	}
+	return elements;
+}
+
+SectionControlPlan resolve_section_control(isobus::DeviceDescriptorObjectPool &pool, const std::vector<std::uint16_t> &sectionElementNumbers)
+{
+	SectionControlPlan plan;
+	if (sectionElementNumbers.empty())
+	{
+		return plan; // Nothing to switch, e.g. a tractor
+	}
+
+	const auto links = get_linked_process_data(pool);
+	auto isSection = [&sectionElementNumbers](std::uint16_t elementNumber) {
+		return std::find(sectionElementNumbers.begin(), sectionElementNumbers.end(), elementNumber) != sectionElementNumbers.end();
+	};
+
+	// A global Setpoint Work State lives above the sections. On a section element it is a per-element setpoint.
+	for (const auto &link : links)
+	{
+		if (link.ddi == SETPOINT_WORK_STATE && link.settable && !isSection(link.elementNumber))
+		{
+			plan.hasSetpointWorkState = true;
+			plan.setpointWorkStateElement = link.elementNumber;
+			break;
+		}
+	}
+
+	// Setpoint condensed work state (DDI 290+). The first linking element wins if several link the same DDI.
+	for (const auto &link : links)
+	{
+		if (is_in(link.ddi, SETPOINT_CONDENSED_FIRST, SETPOINT_CONDENSED_LAST))
+		{
+			plan.condensedTargets.emplace(link.ddi, link.elementNumber);
+		}
+	}
+	if (!plan.condensedTargets.empty())
+	{
+		plan.method = SectionControlMethod::CondensedSetpoint;
+		return plan;
+	}
+
+	// Actual condensed work state (DDI 161+) declared settable, the fallback for implements without setpoint DDIs
+	for (const auto &link : links)
+	{
+		if (is_in(link.ddi, ACTUAL_CONDENSED_FIRST, ACTUAL_CONDENSED_LAST) && link.settable)
+		{
+			plan.condensedTargets.emplace(link.ddi, link.elementNumber);
+		}
+	}
+	if (!plan.condensedTargets.empty())
+	{
+		plan.method = SectionControlMethod::SettableCondensedActual;
+		return plan;
+	}
+
+	// Per-element: Setpoint Work State on each section, or else a settable Actual Work State on each section
+	plan.perElementTargets = get_per_element_targets(links, sectionElementNumbers, SETPOINT_WORK_STATE);
+	if (!plan.perElementTargets.empty())
+	{
+		plan.method = SectionControlMethod::PerElementSetpointWorkState;
+		plan.perElementSetpointDDI = SETPOINT_WORK_STATE;
+		return plan;
+	}
+	plan.perElementTargets = get_per_element_targets(links, sectionElementNumbers, ACTUAL_WORK_STATE);
+	if (!plan.perElementTargets.empty())
+	{
+		plan.method = SectionControlMethod::PerElementActualWorkState;
+		plan.perElementSetpointDDI = ACTUAL_WORK_STATE;
+	}
+	return plan;
+}