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 734e5e2..8f8f29e 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 51956c0..d037604 100644 --- a/src/app.cpp +++ b/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(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(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 70f57e6..a5df679 100644 --- a/src/task_controller.cpp +++ b/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(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 internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -767,6 +811,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; @@ -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(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); + 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 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);