From 679b2933acdc3dc83fd2776db6e583d0979ed6bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 20 Sep 2026 00:07:32 +0200 Subject: [PATCH] Add track control level negotiation (DDI 505/506) and track control state (DDI 515) Negotiates a track control level with implements and switches their track control on and off together with AgOpenGPS's Auto command: 1. The implement reports the levels it supports as a bitmask in DDI 505. 2. The TC writes DDI 506 (an enum, unlike 505) back. Only Level 1 is implemented, so it asks for Level 1 whenever the implement supports it, even if it also advertises Level 2 or 3. 3. The implement's DDI 506 echo completes the negotiation. DDI 515 (TrackControlState) is written only to clients that completed the negotiation, each time AOG's section control mode arrives. The guidance data from the previous PR is not gated on any of this. The DDIs are mapped with find_owning_element_number() from the guidance data mapping, plus an OnChange subscription because the implement reports them. Also announces functionality 27 (Task Controller TRACK server) in PGN 64654 and shows the track state and the implement's supported levels on the VT status page. Co-Authored-By: Claude Sonnet 5 --- docs/PROTOCOL.md | 19 ++++- include/task_controller.hpp | 28 +++++++ readme.md | 1 + src/app.cpp | 46 +++++++++++- src/task_controller.cpp | 141 ++++++++++++++++++++++++++++++++++++ 5 files changed, 232 insertions(+), 3 deletions(-) diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 8610c30..ff37720 100644 --- a/docs/PROTOCOL.md +++ b/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,6 +299,7 @@ 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 @@ -304,10 +307,10 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co | 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. diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 517f17a..7f0912d 100644 --- a/include/task_controller.hpp +++ b/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 client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); diff --git a/readme.md b/readme.md index 0af81c3..875f1d8 100644 --- a/readme.md +++ b/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 diff --git a/src/app.cpp b/src/app.cpp index 675f88c..e4c304a 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -398,7 +398,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(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() @@ -508,6 +514,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 { @@ -1382,6 +1390,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(TrackControlLevel::Level1)) + trackLevels += "L1 "; + if (implementTrackLevels & static_cast(TrackControlLevel::Level2)) + trackLevels += "L2 "; + if (implementTrackLevels & static_cast(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; diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 8f9ca42..79c03ea 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -84,6 +84,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(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; @@ -386,6 +400,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 internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -749,6 +793,51 @@ bool MyTCServer::on_value_command(std::shared_ptr partn } } } + break; + + case static_cast(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(TrackControlLevel::Level1)) ? "yes" : "no") + << " L2=" << ((processDataValue & static_cast(TrackControlLevel::Level2)) ? "yes" : "no") + << " L3=" << ((processDataValue & static_cast(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(TrackControlLevel::Level1)) ? 1 : 0; + send_set_value(partner, + static_cast(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(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(isobus::DataDescriptionIndex::TrackControlState): + { + log("TC") << "Implement reports track control state=" << processDataValue << std::endl; + } + break; } return true; @@ -905,6 +994,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(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(ddi), elementNumber); + if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) + { + send_change_threshold_measurement_command(client.first, ddi, elementNumber, 1); + std::cout << "Subscribed (OnChange) to DDI " << ddi << " (" << entry.to_string() << ") for element " << elementNumber << std::endl; + } + else + { + std::cout << "Mapped (no OnChange) DDI " << ddi << " (" << entry.to_string() << ") to element " << elementNumber << std::endl; + } + } + std::cout << "[" << get_timestamp() << "] Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); } @@ -1026,6 +1151,22 @@ void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std:: } } +void MyTCServer::update_track_control_enabled(bool enabled) +{ + std::lock_guard 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(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 client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex);