Vader tempo having multiple booms

CMakeLists.txt

@@ -29,7 +29,7 @@ FetchContent_MakeAvailable(Boost)
 FetchContent_Declare(
   isobus
   GIT_REPOSITORY https://github.com/Open-Agriculture/AgIsoStack-plus-plus.git
-  GIT_TAG 495eba6653010449d6202165240da7623243f416
+  GIT_TAG 935a3bc16881922c73af5aeddb7f25105f67c1c3
   DOWNLOAD_EXTRACT_TIMESTAMP TRUE)
 FetchContent_MakeAvailable(isobus)
 

include/task_controller.hpp

@@ -50,7 +50,8 @@ class ClientState
 	bool are_measurement_commands_sent() const;
 	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);
+	const std::vector<std::uint16_t> &get_element_numbers_for_ddi(isobus::DataDescriptionIndex ddi) const;
+	void add_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
@@ -60,7 +61,7 @@ class ClientState
 private:
 	isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC
 	bool areMeasurementCommandsSent = false; ///< Whether or not the measurement commands have been sent
-	std::map<isobus::DataDescriptionIndex, std::uint16_t> ddiToElementNumber; ///< Mapping of DDI to element number // TODO: better way to do this?
+	std::map<isobus::DataDescriptionIndex, std::vector<std::uint16_t>> ddiToElementNumbers; ///< Mapping of DDI to element numbers (supports multiple booms)
 
 	std::uint8_t numberOfSections;
 	std::vector<std::uint8_t> sectionSetpointStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed)

src/app.cpp

@@ -134,19 +134,16 @@ bool Application::initialize()
 	static std::uint32_t lastXteTransmit = 0;
 
 	auto packetHandler = [this, tcCF](std::uint8_t src, std::uint8_t pgn, std::span<std::uint8_t> data) {
-		if (src == 0x7F && pgn == 0xFE) // 254 - Steer Data
+		if (src == 0x7F && pgn == 0xE5) // 229 - 64 sections PGN
 		{
-			// TODO: hack to get desired section states. probably want to make a new pgn later when we need more than 16 sections
 			std::vector<bool> sectionStates;
-			for (std::uint8_t i = 0; i < 8; i++)
+			for (std::uint8_t j = 0; j < 8; j++)
 			{
-				sectionStates.push_back(data[6] & (1 << i));
-			}
-			for (std::uint8_t i = 0; i < 8; i++)
-			{
-				sectionStates.push_back(data[7] & (1 << i));
+				for (std::uint8_t i = 0; i < 8; i++)
+				{
+					sectionStates.push_back(data[j] & (1 << i));
+				}
 			}
-
 			tcServer->update_section_states(sectionStates);
 		}
 		else if (src == 0x7F && pgn == 0xF1) // 241 - Section Control

src/logging.cpp

@@ -4,8 +4,10 @@
 
 #include <ctime>
 #include <fstream>
+#include <iomanip>
 #include <iostream>
 #include <settings.hpp>
+#include <sstream>
 #include <string>
 
 class TeeStreambuf : public std::streambuf
@@ -48,6 +50,20 @@ class TeeStreambuf : public std::streambuf
 
 static std::unique_ptr<TeeStreambuf> teeStream;
 
+static std::string get_timestamp()
+{
+	std::time_t now = std::time(nullptr);
+	std::tm localTime;
+	localtime_s(&localTime, &now);
+
+	std::ostringstream oss;
+	oss << std::setfill('0')
+	    << std::setw(2) << localTime.tm_hour << ":"
+	    << std::setw(2) << localTime.tm_min << ":"
+	    << std::setw(2) << localTime.tm_sec;
+	return oss.str();
+}
+
 static void setup_file_logging()
 {
 	// Generate timestamped filename
@@ -74,6 +90,7 @@ class CustomLogger : public isobus::CANStackLogger
 
 	void sink_CAN_stack_log(CANStackLogger::LoggingLevel level, const std::string &text) override
 	{
+		std::cout << "[" << get_timestamp() << "]";
 		switch (level)
 		{
 			case LoggingLevel::Debug:

src/task_controller.cpp

@@ -12,6 +12,7 @@
 #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp"
 #include "isobus/isobus/isobus_task_controller_server.hpp"
 
+#include <algorithm>
 #include <bitset>
 #include <fstream>
 #include <iostream>
@@ -145,23 +146,41 @@ void ClientState::mark_measurement_commands_sent()
 
 std::uint16_t ClientState::get_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const
 {
-	auto it = ddiToElementNumber.find(ddi);
-	if (it != ddiToElementNumber.end())
+	auto it = ddiToElementNumbers.find(ddi);
+	if (it != ddiToElementNumbers.end() && !it->second.empty())
 	{
-		return it->second;
+		return it->second.front(); // Return first element for backwards compatibility
 	}
 	std::cout << "Cached element number not found for DDI " << static_cast<int>(ddi) << std::endl;
 	return 0;
 }
 
-void ClientState::set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber)
+static const std::vector<std::uint16_t> emptyElementNumbers;
+
+const std::vector<std::uint16_t> &ClientState::get_element_numbers_for_ddi(isobus::DataDescriptionIndex ddi) const
+{
+	auto it = ddiToElementNumbers.find(ddi);
+	if (it != ddiToElementNumbers.end())
+	{
+		return it->second;
+	}
+	return emptyElementNumbers;
+}
+
+void ClientState::add_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber)
 {
-	ddiToElementNumber[ddi] = elementNumber;
+	auto &elements = ddiToElementNumbers[ddi];
+	// Avoid duplicates
+	if (std::find(elements.begin(), elements.end(), elementNumber) == elements.end())
+	{
+		elements.push_back(elementNumber);
+	}
 }
 
 bool ClientState::has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const
 {
-	return ddiToElementNumber.find(ddi) != ddiToElementNumber.end();
+	auto it = ddiToElementNumbers.find(ddi);
+	return it != ddiToElementNumbers.end() && !it->second.empty();
 }
 
 bool ClientState::is_element_or_parent_off(std::uint16_t elementNumber) const
@@ -228,8 +247,8 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool &
 MyTCServer::MyTCServer(std::shared_ptr<isobus::InternalControlFunction> internalControlFunction) :
   TaskControllerServer(internalControlFunction,
                        1, // AOG limits to 1 boom
-                       16, // AOG limits to 16 sections of unique width
-                       16, // 16 channels for position based control
+                       64, // AOG limits to 16 sections of unique width but can be 64 by using zones
+                       64, // 64 channels for position based control
                        isobus::TaskControllerOptions()
                          .with_implement_section_control(), // We support section control
                        TaskControllerVersion::SecondEditionDraft)
@@ -496,7 +515,7 @@ void MyTCServer::request_measurement_commands()
 									if (elementObjectChild == processDataObject->get_object_id())
 									{
 										// 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());
+										client.second.add_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;
@@ -543,7 +562,7 @@ void MyTCServer::request_measurement_commands()
 									if (elementObjectChild == processDataObject->get_object_id())
 									{
 										// 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());
+										client.second.add_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))
@@ -595,7 +614,17 @@ void MyTCServer::update_section_states(std::vector<bool> &sectionStates)
 
 			if (i < sectionStates.size())
 			{
-				if (sectionStates[i] != (state.get_section_setpoint_state(i) == SectionState::ON))
+				// Only control sections that are installed (actual state != NOT_INSTALLED)
+				if (state.get_section_actual_state(i) == SectionState::NOT_INSTALLED)
+				{
+					// Ensure setpoint also reflects NOT_INSTALLED
+					if (state.get_section_setpoint_state(i) != SectionState::NOT_INSTALLED)
+					{
+						state.set_section_setpoint_state(i, SectionState::NOT_INSTALLED);
+						requiresUpdate = true;
+					}
+				}
+				else 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;
@@ -635,15 +664,23 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
 	std::uint16_t ddiTarget = static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) + ddiOffset;
 	// 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);
+		// Send to ALL elements that have this DDI (supports multiple booms)
+		for (std::uint16_t elementNumber : clients[client].get_element_numbers_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) && 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);
+			// Send SetpointWorkState to all elements that have it
+			for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
+			{
+				send_set_value(client, static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointWorkState), elementNumber, setpointWorkState ? 1 : 0);
+			}
 			clients[client].set_setpoint_work_state(setpointWorkState);
 		}
 		else if (!clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
@@ -655,7 +692,11 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
 	{
 		if (is_ddi_settable(client, ddiTargetLegacy))
 		{
-			send_set_value(client, ddiTargetLegacy, clients[client].get_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTargetLegacy)), value);
+			// Send to ALL elements that have this legacy DDI
+			for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddiTargetLegacy)))
+			{
+				send_set_value(client, ddiTargetLegacy, elementNumber, value);
+			}
 		}
 		else
 		{
@@ -671,7 +712,11 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
 
 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);
+	// Send SectionControlState to all elements that have it
+	for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(isobus::DataDescriptionIndex::SectionControlState))
+	{
+		send_set_value(client, static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SectionControlState), elementNumber, enabled ? 1 : 0);
+	}
 }
 
 bool MyTCServer::is_ddi_settable(std::shared_ptr<isobus::ControlFunction> client, std::uint16_t ddi)