Section Control fixes

include/task_controller.hpp

@@ -9,6 +9,7 @@
 
 #pragma once
 
+#include "isobus/isobus/isobus_data_dictionary.hpp"
 #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"
@@ -36,6 +37,8 @@ class ClientState
 	std::uint8_t get_number_of_sections() const;
 	std::uint8_t get_section_setpoint_state(std::uint8_t section) const;
 	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 is_any_section_setpoint_on() const;
 	bool get_setpoint_work_state() const;
 	void set_setpoint_work_state(bool state);
@@ -48,6 +51,11 @@ class ClientState
 	void mark_measurement_commands_sent();
 	std::uint16_t get_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const;
 	void set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber);
+	bool has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const;
+	bool is_element_or_parent_off(std::uint16_t elementNumber) const; ///< Recursively checks if element or any parent is off
+	// 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;
 
 private:
 	isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC
@@ -57,8 +65,10 @@ class ClientState
 	std::uint8_t numberOfSections;
 	std::vector<std::uint8_t> sectionSetpointStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed)
 	std::vector<std::uint8_t> sectionActualStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed)
-	bool setpointWorkState = false; ///< The overall work state desired
+	std::vector<std::uint16_t> sectionToElementNumber; // Maps section index to element number for hierarchy checking
+	bool setpointWorkState = false; ///< The overall work state desired (DDI 289)
 	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
 };
 
@@ -91,6 +101,7 @@ 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/app.cpp

@@ -41,6 +41,8 @@ bool Application::initialize()
 		return false;
 	}
 
+	isobus::CANNetworkManager::CANNetwork.get_configuration().set_number_of_packets_per_cts_message(255);
+
 	isobus::NAME ourNAME(0);
 
 	//! Make sure you change these for your device!!!!

src/main.cpp

@@ -116,8 +116,8 @@ class ArgumentProcessor
 			std::cout << "Options:\n";
 			std::cout << "  --help\t\tShow this help message\n";
 			std::cout << "  --version\t\tShow the version of the application\n";
-			std::cout << "  --adapter=<driver>\tSelect the CAN driver\n";
-			std::cout << "  --channel=<channel>\tSelect the CAN channel\n";
+			std::cout << "  --can_adapter=<driver>\tSelect the CAN driver\n";
+			std::cout << "  --can_channel=<channel>\tSelect the CAN channel\n";
 			std::cout << "  --log_level=<level>\tSet the log level (debug, info, warning, error, critical)\n";
 			std::cout << "  --log2file\t\tLog to file\n";
 			exit(0);

src/task_controller.cpp

@@ -21,6 +21,7 @@ void ClientState::set_number_of_sections(std::uint8_t number)
 	numberOfSections = number;
 	sectionSetpointStates.resize(number);
 	sectionActualStates.resize(number);
+	sectionToElementNumber.resize(number, 0); // Initialize all sections mapped to element 0 by default
 }
 
 void ClientState::set_section_setpoint_state(std::uint8_t section, std::uint8_t state)
@@ -39,6 +40,23 @@ void ClientState::set_section_actual_state(std::uint8_t section, std::uint8_t st
 	}
 }
 
+std::uint16_t ClientState::get_element_number_for_section(std::uint8_t section) const
+{
+	if (section < numberOfSections && section < sectionToElementNumber.size())
+	{
+		return sectionToElementNumber[section];
+	}
+	return 0;
+}
+
+void ClientState::set_element_number_for_section(std::uint8_t section, std::uint16_t elementNumber)
+{
+	if (section < numberOfSections && section < sectionToElementNumber.size())
+	{
+		sectionToElementNumber[section] = elementNumber;
+	}
+}
+
 std::uint8_t ClientState::get_number_of_sections() const
 {
 	return numberOfSections;
@@ -57,6 +75,12 @@ 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
+		std::uint16_t elementNumber = get_element_number_for_section(section);
+		if (is_element_or_parent_off(elementNumber))
+		{
+			return SectionState::OFF;
+		}
 		return sectionActualStates[section];
 	}
 	return SectionState::NOT_INSTALLED;
@@ -135,6 +159,72 @@ void ClientState::set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, s
 	ddiToElementNumber[ddi] = elementNumber;
 }
 
+bool ClientState::has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const
+{
+	return ddiToElementNumber.find(ddi) != ddiToElementNumber.end();
+}
+
+bool ClientState::is_element_or_parent_off(std::uint16_t elementNumber) const
+{
+	// Check if this element is off
+	bool elementWorkState;
+	if (try_get_element_work_state(elementNumber, elementWorkState) && !elementWorkState)
+	{
+		return true; // Element is off
+	}
+
+	// Find the parent element(s) by searching through the pool
+	// Use const_cast to access non-const methods on the pool from a const context
+	auto &nonConstPool = const_cast<isobus::DeviceDescriptorObjectPool &>(pool);
+	for (std::uint32_t i = 0; i < nonConstPool.size(); i++)
+	{
+		auto object = nonConstPool.get_object_by_index(i);
+		if (object->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceElement)
+		{
+			auto elementObject = std::dynamic_pointer_cast<isobus::task_controller_object::DeviceElementObject>(object);
+			// Check if this element has elementNumber as a child
+			for (std::uint16_t childId : elementObject->get_child_object_ids())
+			{
+				// Find the child object
+				for (std::uint32_t j = 0; j < nonConstPool.size(); j++)
+				{
+					auto childObject = nonConstPool.get_object_by_index(j);
+					if (childObject && childObject->get_object_id() == childId)
+					{
+						if (childObject->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceElement)
+						{
+							auto childElementObject = std::dynamic_pointer_cast<isobus::task_controller_object::DeviceElementObject>(childObject);
+							if (childElementObject && childElementObject->get_element_number() == elementNumber)
+							{
+								// Found the parent, recursively check if parent or its parents are off
+								return is_element_or_parent_off(elementObject->get_element_number());
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	return false; // No parent found or no parents are off
+}
+
+void ClientState::set_element_work_state(std::uint16_t elementNumber, bool isWorking)
+{
+	elementWorkStates[elementNumber] = isWorking;
+}
+
+bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const
+{
+	auto it = elementWorkStates.find(elementNumber);
+	if (it != elementWorkStates.end())
+	{
+		isWorking = it->second;
+		return true;
+	}
+	return false;
+}
+
 MyTCServer::MyTCServer(std::shared_ptr<isobus::InternalControlFunction> internalControlFunction) :
   TaskControllerServer(internalControlFunction,
                        1, // AOG limits to 1 boom
@@ -329,7 +419,9 @@ bool MyTCServer::on_value_command(std::shared_ptr<isobus::ControlFunction> partn
 
 			for (std::uint_fast8_t i = 0; i < NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; i++)
 			{
-				clients[partner].set_section_actual_state(i + sectionIndexOffset, (processDataValue >> (2 * i)) & 0x03);
+				std::uint8_t sectionState = ((processDataValue >> (2 * i)) & 0x03);
+				clients[partner].set_section_actual_state(i + sectionIndexOffset, sectionState);
+				clients[partner].set_element_number_for_section(i + sectionIndexOffset, elementNumber);
 			}
 		}
 		break;
@@ -342,7 +434,8 @@ bool MyTCServer::on_value_command(std::shared_ptr<isobus::ControlFunction> partn
 
 		case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualWorkState):
 		{
-			clients[partner].set_setpoint_work_state(processDataValue == 1);
+			// Store the work state per element rather than globally
+			clients[partner].set_element_work_state(elementNumber, processDataValue == 1);
 		}
 	}
 
@@ -379,7 +472,9 @@ void MyTCServer::request_measurement_commands()
 					auto processDataObject = std::dynamic_pointer_cast<isobus::task_controller_object::DeviceProcessDataObject>(object);
 					if (processDataObject->get_ddi() == static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualWorkState) ||
 					    (processDataObject->get_ddi() >= static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualCondensedWorkState1_16) &&
-					     processDataObject->get_ddi() <= static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualCondensedWorkState241_256)))
+					     processDataObject->get_ddi() <= static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualCondensedWorkState241_256)) ||
+					    (processDataObject->get_ddi() >= static_cast<std::uint16_t>(isobus::DataDescriptionIndex::CondensedSectionOverrideState1_16) &&
+					     processDataObject->get_ddi() <= static_cast<std::uint16_t>(isobus::DataDescriptionIndex::CondensedSectionOverrideState241_256)))
 					{
 						// Loop over all objects to find the elements that are the parents of the actual condensed work state objects
 						for (std::uint32_t j = 0; j < client.second.get_pool().size(); j++)
@@ -394,10 +489,15 @@ void MyTCServer::request_measurement_commands()
 									{
 										// TODO: This is a bit of a hack, but it works for now
 										client.second.set_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(processDataObject->get_ddi()), elementObject->get_element_number());
+										const auto &entryB = isobus::DataDictionary::get_entry(processDataObject->get_ddi());
+										std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") to element "
+										          << elementObject->get_element_number() << std::endl;
 
 										if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange))
 										{
 											send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1);
+											std::cout << "Subscribed (OnChange) to DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") for element "
+											          << elementObject->get_element_number() << std::endl;
 										}
 										if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::TimeInterval))
 										{
@@ -436,10 +536,18 @@ void MyTCServer::request_measurement_commands()
 									{
 										// TODO: This is a bit of a hack, but it works for now
 										client.second.set_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(processDataObject->get_ddi()), elementObject->get_element_number());
+										const auto &entryB = isobus::DataDictionary::get_entry(processDataObject->get_ddi());
 
 										if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange))
 										{
 											send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1);
+											std::cout << "Subscribed (OnChange) to DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") for element "
+											          << elementObject->get_element_number() << std::endl;
+										}
+										else
+										{
+											std::cout << "Mapped (no OnChange) DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") to element "
+											          << elementObject->get_element_number() << std::endl;
 										}
 									}
 								}
@@ -488,7 +596,7 @@ void MyTCServer::update_section_states(std::vector<bool> &sectionStates)
 		}
 		if (requiresUpdate)
 		{
-			std::uint8_t ddiOffset = state.get_number_of_sections() / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE;
+			std::uint8_t ddiOffset = (state.get_number_of_sections() - 1) / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE;
 			send_section_setpoint_states(client.first, ddiOffset);
 		}
 	}
@@ -515,19 +623,62 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
 		value |= (clients[client].get_section_setpoint_state(sectionOffset + i) << (2 * i));
 	}
 
+	// Modern ECU? (DDI 290  SetpointCondensedWorkState1_16 exists)
 	std::uint16_t ddiTarget = static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) + ddiOffset;
-	std::uint16_t elementNumber = clients[client].get_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTarget));
-	send_set_value(client, ddiTarget, elementNumber, value);
+	// Legacy ECU? (DDI 161  ActualCondensedWorkState1_16 exists and Settable)
+	std::uint16_t ddiTargetLegacy = static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualCondensedWorkState1_16) + ddiOffset;
+	if (clients[client].has_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTarget)))
+	{
+		std::uint16_t elementNumber = clients[client].get_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTarget));
+		send_set_value(client, ddiTarget, elementNumber, value);
 
-	bool setpointWorkState = clients[client].is_any_section_setpoint_on();
-	if ((clients[client].get_setpoint_work_state() != setpointWorkState))
+		bool setpointWorkState = clients[client].is_any_section_setpoint_on();
+		if ((clients[client].get_setpoint_work_state() != setpointWorkState) && clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
+		{
+			send_set_value(client, static_cast<std::uint16_t>(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))
+		{
+			std::cout << "[TC Server] DDI 289 (SetpointWorkState) not available!" << std::endl;
+		}
+	}
+	else if (clients[client].has_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTargetLegacy)))
 	{
-		send_set_value(client, static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointWorkState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState), setpointWorkState ? 1 : 0);
-		clients[client].set_setpoint_work_state(setpointWorkState);
+		if (is_ddi_settable(client, ddiTargetLegacy))
+		{
+			send_set_value(client, ddiTargetLegacy, clients[client].get_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTargetLegacy)), value);
+		}
+		else
+		{
+			std::cout << "[TC Server] Legacy DDI " << ddiTargetLegacy << " (ActualCondensedWorkState) is not settable!" << std::endl;
+		}
+		return;
+	}
+	else
+	{
+		std::cout << "[TC Server] Neither condensed nor controllable-actual work state supported Missing DDI 290 and 141!" << std::endl;
 	}
 }
 
 void MyTCServer::send_section_control_state(std::shared_ptr<isobus::ControlFunction> client, bool enabled)
 {
 	send_set_value(client, static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SectionControlState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState), enabled ? 1 : 0);
 }
+
+bool MyTCServer::is_ddi_settable(std::shared_ptr<isobus::ControlFunction> client, std::uint16_t ddi)
+{
+	for (std::uint32_t i = 0; i < clients[client].get_pool().size(); i++)
+	{
+		auto object = clients[client].get_pool().get_object_by_index(i);
+		if (object->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceProcessData)
+		{
+			auto processDataObject = std::dynamic_pointer_cast<isobus::task_controller_object::DeviceProcessDataObject>(object);
+			if (processDataObject->get_ddi() == ddi)
+			{
+				return processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable);
+			}
+		}
+	}
+	return false;
+}