Add track control level negotiation (DDI 505/506) and track control state (DDI 515)

docs/PROTOCOL.md

@@ -285,10 +285,12 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac
 | `0xCB00` (Process Data) | 2 s | TC | ISO 11783-10 B.8.1 Task Controller Status. Status byte bit 1 = task totals active. |
 | `0x1F903` (NMEA2000 XTE) | 1 Hz | TC | Cross-track error, derived from AOG's guidance-line deviation PGN. |
 | `0xFEE8` (PGN 65256 Speed/Direction) | 100 ms | TECU | Ground/Wheel/Machine-selected speed + machine direction, J1939 format. Only when TECU enabled. |
+| `0xFC8E` (Control Function Functionalities) | At claim + periodic | TC | Announces TaskControllerBasicServer (v1), TaskControllerSectionControlServer (v1, 1 boom / 64 sections) and functionality 27, the Task Controller TRACK server (v1). See §5.4.2. |
 | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). |
 | NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. |
 | GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | AOG's GPS fix quality (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` when no fresh fix quality is available. |
 | Guidance track data (DDI 507-513, via `0xCB00` Process Data) | 250 ms, while AOG has a valid track | TC | Track number, adjacent tracks, reference line, swath width and line deviation from AOG's PGN `0xF4`/`0xF2`, sent to each client whose DDOP declares them as settable. See §5.4.1. |
+| Track control level and state (DDI 506, 515, via `0xCB00` Process Data) | Level once, state on each AOG section-control mode packet | TC | The track control level the TC picked, then the on/off state — only to clients that completed the negotiation. See §5.4.2. |
 
 The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements.
 
@@ -297,17 +299,18 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co
 - **Device Descriptor Object Pool (DDOP)** uploads from clients (stored per client).
 - **Condensed actual work-state DDIs** (160, 161, 290, plus the extended range 16001–16016 per the standard): mapped into the per-client section model and forwarded to AgIO/AgValonia as PGN `0xF0`.
 - **Section control state DDI**: tracked per client.
+- **Track control DDIs (505, 506, 515)**: negotiated and tracked per client — see §5.4.2.
 - **Process data acknowledges (PDACK)**: logged.
 
 ### 5.4 ISOBUS feature scope
 
 | Capability | Value |
 |---|---|
 | ISO 11783-10 version | 2 (Second Edition) |
-| Generation | 1 (TC-SC) |
+| Generation | 1 (TC-SC), plus TRACK (Track Control) Level 1 |
 | Max booms | 1 |
 | Max sections | 64 |
-| Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states), plus speed/distance/guidance DDIs from the tractor side |
+| Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states); 505 / 506 / 515 (track control, see §5.4.2); 507-511 / 513 / 514 (guidance data, see §5.4.1); plus speed/distance/guidance DDIs from the tractor side |
 
 #### 5.4.1 Guidance data sent to implements
 
@@ -325,6 +328,18 @@ The TC pushes guidance data to each client whose DDOP declares the DDI as settab
 
 "Valid track" means the TC holds an accepted PGN `0xF4` payload with the valid flag set, a non-zero reference line ID, and an open field (PGN `0xF3`) to scope the ID to. Since AOG only sends `0xF4` on change, validity is *not* cleared just because no new `0xF4` has arrived; it is cleared only by an explicit "guidance off" packet, by the field closing, or by AOG disconnecting entirely (no packets of any kind for 3 s).
 
+#### 5.4.2 Track control level negotiation
+
+At TC control-function claim, PGN 64654 (Control Function Functionalities, source = the TC's own address — see §5.2) announces functionality 27, the Task Controller TRACK server, to tell implements this TC supports track control.
+
+For each client whose DDOP declares DDI 505 and 506 the TC negotiates a level:
+
+1. The implement reports DDI 505 (`SupportedTrackControlLevels`) as a **bitmask** (bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3).
+2. The TC writes DDI 506 (`SetpointTrackControlLevel`) back as an **enum** (`0` = no common level, `1` = Level 1, `2` = Level 2, `3` = Level 3). Only Level 1 is implemented, so this is `1` whenever the implement supports Level 1, even if it also advertises Level 2 or 3.
+3. The implement's DDI 506 echo completes the negotiation, if it echoes `1`.
+
+Only DDI 515 (`TrackControlState`: `0` = manual/off, `1` = automatic/on) depends on the negotiation: it is written to clients that completed it, each time AOG's section control mode (PGN `0xF1`, see §2.5) arrives. The guidance data in §5.4.1 is sent whether or not a client negotiated a level.
+
 ### 5.5 Virtual Terminal UI
 
 The roughly 12 KB VT object pool is embedded in the executable, so deployment does not require a separate `AOG_TC.iop` file. The committed `src/AOG_TC.iop` remains its build-time source of truth, while `src/AOG_TC.iop.h` contains the ISO-Designer-generated object IDs and authored geometry.

include/task_controller.hpp

@@ -30,6 +30,16 @@ enum SectionState : std::uint8_t
 	NOT_INSTALLED = 3 ///< Section is not installed
 };
 
+/// @brief Track control levels, as the bits an implement sets in DDI 505 for the levels it supports.
+/// Only Level 1 is implemented by the TC.
+enum class TrackControlLevel : std::uint8_t
+{
+	None = 0,
+	Level1 = 1,
+	Level2 = 2,
+	Level3 = 4
+};
+
 class ClientState
 {
 public:
@@ -69,6 +79,16 @@ class ClientState
 	/// @returns The sequence number to announce.
 	std::uint32_t update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId);
 
+	// Track control level negotiation. The implement reports the levels it supports in DDI 505,
+	// the TC answers by writing the level it wants to use to DDI 506, and the implement's echo of
+	// that completes the negotiation.
+	int get_supported_track_control_levels() const;
+	void set_supported_track_control_levels(int levels);
+	bool is_track_control_level_sent() const;
+	void set_track_control_level_sent(bool sent);
+	bool is_track_negotiation_complete() const;
+	void set_track_negotiation_complete(bool complete);
+
 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
@@ -88,6 +108,9 @@ class ClientState
 	std::int32_t lastSentTrackNumber = 0; ///< Last track number announced, for DDI 507 change detection
 	std::uint32_t lastSentReferenceLineId = 0; ///< Last reference line ID announced, for DDI 507 change detection
 	std::uint32_t guidanceTrackSequenceNumber = 0; ///< Per-client DDI 507 sequence number
+	int supportedTrackControlLevels = 0; ///< Raw DDI 505 bitmask from the implement (bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3)
+	bool trackControlLevelSent = false; ///< Whether DDI 506 has been written
+	bool trackNegotiationComplete = false; ///< Whether the implement confirmed the level written to DDI 506
 };
 
 // Create the task controller server object, this will handle all the ISOBUS communication for us
@@ -135,6 +158,11 @@ class MyTCServer : public isobus::TaskControllerServer
 	/// @param lineDeviationMm Deviation from the guidance line in mm
 	void send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm);
 
+	/// @brief Writes the track control state (DDI 515) to every client that has finished negotiating
+	/// a track control level and declares that DDI.
+	/// @param enabled true = automatic, false = manual/off
+	void update_track_control_enabled(bool enabled);
+
 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);

readme.md

@@ -90,6 +90,7 @@ The application logs the detected VT version, screen size, softkey dimensions, a
 - **Maximum Booms:** 1
 - **Maximum Sections:** 64 (supports both individual sections and zone-based control)
 - **Section Control:** Generation 1 (TC-SC) with support for DDI 160/161/290
+- **Track Control:** Level 1 — negotiates the level with the implement (DDI 505/506) and switches its track control on and off (DDI 515) together with AgOpenGPS's Auto command
 
 ### Guidance data sent to implements
 

src/app.cpp

@@ -399,7 +399,13 @@ void Application::setup_task_controller_server()
 	  1,
 	  true);
 	tcFunctionalities->set_task_controller_section_control_server_option_state(1, 64);
-	log("Init") << "TC announced TC-BAS and TC-SC (1 boom / 64 sections) via PGN 64654" << std::endl;
+
+	// Announce the Task Controller TRACK Server (functionality 27), telling implements this TC
+	// negotiates a track control level. AgIsoStack has no enumerator for it yet, so cast the value.
+	constexpr auto TASK_CONTROLLER_TRACK_SERVER = static_cast<isobus::ControlFunctionFunctionalities::Functionalities>(27);
+	tcFunctionalities->set_functionality_is_supported(TASK_CONTROLLER_TRACK_SERVER, 1, true);
+
+	log("Init") << "TC announced TC-BAS, TC-SC (1 boom / 64 sections) and TC-TRACK via PGN 64654" << std::endl;
 }
 
 void Application::setup_tecu_interfaces()
@@ -509,6 +515,8 @@ void Application::setup_udp_connections()
 			std::uint8_t sectionControlState = data[0];
 			log() << "Received request from AOG to change section control state to " << (sectionControlState == 1 ? "enabled" : "disabled") << std::endl;
 			tcServer->update_section_control_enabled(sectionControlState == 1);
+			// Track control is separate from section control, even though the same AOG Auto command drives both.
+			tcServer->update_track_control_enabled(sectionControlState == 1);
 		}
 		else if (pgn == 0xEF) // 239 - Machine Data
 		{
@@ -1433,6 +1441,42 @@ void Application::update_vt_status_strings(bool aogConnected)
 	mainImplementStatus << "Name             " << implementDisplayName << '\n'
 	                    << "Sections         " << totalSections << '\n'
 	                    << "Section control  " << sectionControl;
+
+	// Live guidance track from AOG, plus the track control levels the implement reports (DDI 505)
+	{
+		std::string trackState = "OFF";
+		if (!aogConnected)
+		{
+			trackState = "n/a";
+		}
+		else if (currentTrackContext.valid)
+		{
+			trackState = "ref:" + std::to_string(currentTrackContext.guidanceReferenceLineId) +
+			  " track:" + std::to_string(currentTrackContext.actualTrackNumber);
+		}
+
+		int implementTrackLevels = 0;
+		for (const auto &client : clients)
+		{
+			if (client.second.get_supported_track_control_levels() != 0)
+			{
+				implementTrackLevels = client.second.get_supported_track_control_levels();
+				break;
+			}
+		}
+		std::string trackLevels;
+		if (implementTrackLevels & static_cast<int>(TrackControlLevel::Level1))
+			trackLevels += "L1 ";
+		if (implementTrackLevels & static_cast<int>(TrackControlLevel::Level2))
+			trackLevels += "L2 ";
+		if (implementTrackLevels & static_cast<int>(TrackControlLevel::Level3))
+			trackLevels += "L3 ";
+		if (trackLevels.empty())
+			trackLevels = "NONE";
+
+		mainImplementStatus << "\nTrack state      " << trackState
+		                    << "\nTrack levels     " << trackLevels;
+	}
 	send_vt_string_if_changed(VTSectionsFromAOGS, mainImplementStatus.str());
 
 	std::ostringstream distanceText;

src/task_controller.cpp

@@ -85,6 +85,20 @@ static bool is_guidance_data_ddi(std::uint16_t ddi)
 	}
 }
 
+// The DDIs used to negotiate a track control level and report the control state.
+static bool is_track_control_ddi(std::uint16_t ddi)
+{
+	switch (static_cast<isobus::DataDescriptionIndex>(ddi))
+	{
+		case isobus::DataDescriptionIndex::SupportedTrackControlLevels:
+		case isobus::DataDescriptionIndex::SetpointTrackControlLevel:
+		case isobus::DataDescriptionIndex::TrackControlState:
+			return true;
+		default:
+			return false;
+	}
+}
+
 void ClientState::set_number_of_sections(std::uint8_t number)
 {
 	numberOfSections = number;
@@ -387,6 +401,36 @@ std::uint32_t ClientState::update_guidance_track_sequence(std::int32_t trackNumb
 	return guidanceTrackSequenceNumber;
 }
 
+int ClientState::get_supported_track_control_levels() const
+{
+	return supportedTrackControlLevels;
+}
+
+void ClientState::set_supported_track_control_levels(int levels)
+{
+	supportedTrackControlLevels = levels;
+}
+
+bool ClientState::is_track_control_level_sent() const
+{
+	return trackControlLevelSent;
+}
+
+void ClientState::set_track_control_level_sent(bool sent)
+{
+	trackControlLevelSent = sent;
+}
+
+bool ClientState::is_track_negotiation_complete() const
+{
+	return trackNegotiationComplete;
+}
+
+void ClientState::set_track_negotiation_complete(bool complete)
+{
+	trackNegotiationComplete = complete;
+}
+
 MyTCServer::MyTCServer(std::shared_ptr<isobus::InternalControlFunction> internalControlFunction,
                        isobus::TaskControllerServer::TaskControllerVersion version) :
   TaskControllerServer(internalControlFunction,
@@ -767,6 +811,51 @@ bool MyTCServer::on_value_command(std::shared_ptr<isobus::ControlFunction> partn
 				}
 			}
 		}
+		break;
+
+		case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SupportedTrackControlLevels):
+		{
+			// DDI 505 is a BITMASK: bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3, so a value
+			// of 3 means Level 1 and 2 are supported (not "Level 3"). DDI 506, which the TC writes
+			// in answer, is an ENUM instead: 0 = no common level, 1 = Level 1, 2 = Level 2, 3 = Level 3.
+			log("TC") << "Implement reports supported track control levels=" << processDataValue
+			          << " (L1=" << ((processDataValue & static_cast<int>(TrackControlLevel::Level1)) ? "yes" : "no")
+			          << " L2=" << ((processDataValue & static_cast<int>(TrackControlLevel::Level2)) ? "yes" : "no")
+			          << " L3=" << ((processDataValue & static_cast<int>(TrackControlLevel::Level3)) ? "yes" : "no") << ")" << std::endl;
+			auto &state = clients[partner];
+			state.set_supported_track_control_levels(processDataValue);
+
+			// Only Level 1 is implemented, so ask for it whenever the implement supports it, even if it
+			// also advertises Level 2 or 3.
+			if (!state.is_track_control_level_sent() &&
+			    state.has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTrackControlLevel))
+			{
+				const std::int32_t requestedLevel = (processDataValue & static_cast<int>(TrackControlLevel::Level1)) ? 1 : 0;
+				send_set_value(partner,
+				               static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointTrackControlLevel),
+				               state.get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTrackControlLevel),
+				               requestedLevel);
+				state.set_track_control_level_sent(true);
+				log("TC") << "Wrote track control level " << requestedLevel << std::endl;
+			}
+		}
+		break;
+
+		case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointTrackControlLevel):
+		{
+			// The implement's echo of the level we wrote completes the negotiation.
+			const bool agreed = (processDataValue == 1);
+			log("TC") << "Implement confirmed track control level " << processDataValue
+			          << (agreed ? " — negotiation complete" : " — no common level") << std::endl;
+			clients[partner].set_track_negotiation_complete(agreed);
+		}
+		break;
+
+		case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::TrackControlState):
+		{
+			log("TC") << "Implement reports track control state=" << processDataValue << std::endl;
+		}
+		break;
 	}
 
 	return true;
@@ -923,6 +1012,42 @@ void MyTCServer::request_measurement_commands()
 				}
 			}
 
+			// Map the track control DDIs. Unlike the guidance data above, the implement reports these
+			// (supported levels, the echo of the level we write, its control state), so subscribe to them.
+			for (std::uint32_t i = 0; i < client.second.get_pool().size(); i++)
+			{
+				auto object = client.second.get_pool().get_object_by_index(i);
+				if (!object || object->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceProcessData)
+				{
+					continue;
+				}
+
+				auto processDataObject = std::dynamic_pointer_cast<isobus::task_controller_object::DeviceProcessDataObject>(object);
+				if (!processDataObject || !is_track_control_ddi(processDataObject->get_ddi()))
+				{
+					continue;
+				}
+
+				std::uint16_t elementNumber = 0;
+				if (!find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber))
+				{
+					continue;
+				}
+
+				const auto ddi = processDataObject->get_ddi();
+				const auto &entry = isobus::DataDictionary::get_entry(ddi);
+				client.second.set_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(ddi), elementNumber);
+				if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange))
+				{
+					send_change_threshold_measurement_command(client.first, ddi, elementNumber, 1);
+					log() << "Subscribed (OnChange) to DDI " << ddi << " (" << entry.to_string() << ") for element " << elementNumber << std::endl;
+				}
+				else
+				{
+					log() << "Mapped (no OnChange) DDI " << ddi << " (" << entry.to_string() << ") to element " << elementNumber << std::endl;
+				}
+			}
+
 			log() << "Measurement commands sent." << std::endl;
 			client.second.mark_measurement_commands_sent();
 		}
@@ -1044,6 +1169,22 @@ void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std::
 	}
 }
 
+void MyTCServer::update_track_control_enabled(bool enabled)
+{
+	std::lock_guard<std::recursive_mutex> lock(clientsMutex);
+	constexpr auto DDI = isobus::DataDescriptionIndex::TrackControlState;
+	for (auto &client : clients)
+	{
+		// Only clients that agreed on a track control level get commanded.
+		if (client.second.is_track_negotiation_complete() && client.second.has_element_number_for_ddi(DDI))
+		{
+			// DDI 515 values: 0 = manual/off, 1 = automatic/on
+			send_set_value(client.first, static_cast<std::uint16_t>(DDI), client.second.get_element_number_for_ddi(DDI), enabled ? 1 : 0);
+			log("TC") << "Track control state=" << (enabled ? "On" : "Off") << std::endl;
+		}
+	}
+}
+
 void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset)
 {
 	std::lock_guard<std::recursive_mutex> lock(clientsMutex);