From 7d05e40bdb0f88875e7e5c1cad8521bab2ff3bd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 19 Sep 2026 23:41:44 +0200 Subject: [PATCH 01/22] Send GNSS quality (DDI 514) to implements Parses the fix-quality byte from AOG's GPS/IMU PGN 0xD6 (source 0x7C) and pushes it to every client whose DDOP declares DDI 514 (GNSSQuality), every 250 ms. - AOG fix values 0-8 are forwarded unchanged (they are the NMEA 2000 GNSS Method values DDI 514 uses, including 6=Estimated, 7=Manual, 8=Simulated). Values above 8 and a stale/missing 0xD6 map to 1 (weakest real fix) rather than 0, since some implements gate TRACK and section control on GNSS quality being non-zero. - DDI 514 is mapped to its element in request_measurement_commands() alongside the other DDI mappings; mapping only, no OnChange subscription, since it is a value the TC pushes rather than one it reads back. Only implements that declare DDI 514 as settable are mapped, since a non-settable DDI cannot accept a Value command. - docs/PROTOCOL.md and the readme document the new PGN and DDI. The readme gets its own "Guidance data sent to implements" subsection so follow-up guidance PRs append bullets there instead of all editing the shared capabilities list. Co-Authored-By: Claude Sonnet 5 --- docs/PROTOCOL.md | 13 ++++++- include/app.hpp | 3 ++ include/task_controller.hpp | 4 ++ readme.md | 6 +++ src/app.cpp | 48 ++++++++++++++++++++++++ src/task_controller.cpp | 75 +++++++++++++++++++++++++++++++++++++ 6 files changed, 147 insertions(+), 2 deletions(-) diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index e9d22e8..f7bf925 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -48,7 +48,7 @@ Every packet — both directions — uses the same frame: 5+N 1 Checksum Sum of bytes [Source .. last payload byte], mod 256 ``` -Total wire size is `N + 6` bytes. The maximum payload is currently 250 bytes (limited by the TC's 512-byte receive buffer; in practice the largest PGN in use is 8 bytes). +Total wire size is `N + 6` bytes. The maximum payload is currently 250 bytes (limited by the TC's 512-byte receive buffer). Most PGNs are ≤10 bytes; the outlier is `0xD6` (GPS/IMU data, see §2.5), which needs at least 39 bytes today. **Checksum**: the TC currently does **not** validate inbound checksums (the verification code is present but commented out in `udp_connections.cpp`). Clients **should still compute and include a correct checksum** so that future TC versions, or third-party listeners, can validate. @@ -58,6 +58,7 @@ Source byte identifies the logical sender of a frame. The conventions used today | Source | Logical sender | |---|---| +| `0x7C` (124) | **AOG's GPS/IMU submodule** — sends PGN `0xD6` only (see §2.5). | | `0x7F` (127) | **AgIO / AgValonia** (the GUI/host application) | | `0x80` (128) | **AOG-TaskController** itself | @@ -87,11 +88,12 @@ If no NIC matches, the TC falls back to loopback (`127.0.0.1`) — useful for lo ### 2.5 PGNs inbound (client → TC) -All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`. +All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`, except `0xD6` (GPS/IMU data), which comes from AOG's GPS submodule at source `0x7C`. | PGN | Name | Length | Payload | |---|---|---|---| | `0xC9` (201) | Subnet detection | 5 | `[0xC9, 0xC9, IP0, IP1, IP2]` | +| `0xD6` (214) | GPS/IMU data | variable (≥39 used) | Only byte 38 (fix quality) is parsed today; rest of frame is currently unused. Source `0x7C`. | | `0xE5` (229) | Section states (64 sections) | 8 | Bitfield: bit `8·j + i` of byte `j` is section `(8j + i)` ON/OFF | | `0xF1` (241) | Section control mode | 1 | `[mode]` where `1` = enabled, `0` = disabled | | `0xF2` (242) | Process data | 6 | `[DDI_lo, DDI_hi, val0, val1, val2, val3]` — DDI is little-endian `uint16`; value is little-endian `int32` | @@ -102,6 +104,12 @@ Tells the TC which `/24` subnet AgIO/AgValonia lives on. The first two payload b On receipt the TC sets `settings.subnet = [IP0, IP1, IP2]`, closes the main socket, re-runs NIC enumeration, and rebinds. Useful for plug-and-play scenarios where the host may move between subnets. +#### `0xD6` — GPS/IMU data + +Sent from AOG's GPS submodule, source `0x7C` (not `0x7F`). The TC only reads byte 38: AOG's fix-quality code (`0`=invalid, `1`=GPS, `2`=DGPS, `3`=PPS, `4`=RTK Fixed, `5`=RTK Float, `6`=Estimated, `7`=Manual, `8`=Simulated — the NMEA 2000 GNSS Method values DDI 514 uses). Values `0`–`8` are forwarded unchanged to implements as DDI 514 (GNSSQuality) — see §5.2. + +Two fallback cases both resolve to `1` (weakest real GNSS fix), not `0` (No GNSS): AOG reporting a value above `8` (not a defined GNSS Method), and no fresh `0xD6` (AOG doesn't always send this PGN at all — e.g. Simulator mode — and if none has arrived within 2 s the last value is treated as stale). `0` is deliberately avoided as a fallback because some implements gate TRACK/section control on GNSS quality being non-zero. + #### `0xE5` — Section states Reports the *actual* state of up to 64 sections. 8 bytes = 64 bits, one bit per section. The TC forwards these to the connected ISOBUS implement via the appropriate condensed work-state DDIs (DDI 160/161/290). @@ -252,6 +260,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `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 | 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. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. diff --git a/include/app.hpp b/include/app.hpp index d74de11..8bc4ce5 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -101,6 +101,9 @@ class Application std::uint32_t lastTCStatusTransmit = 0; std::int32_t lastSpeedValue = 0; std::int32_t lastXteValue = 0; + std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated + std::uint32_t lastGnssQualityMs = 0; ///< Timestamp of last PGN 0xD6 fix-quality update (0 = never received) + static constexpr std::uint32_t GNSS_QUALITY_TIMEOUT_MS = 2000; ///< No PGN 0xD6 for this long = fix quality unknown std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; std::uint32_t vtDisconnectedSinceMs = 0; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 705605e..839a4fc 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -116,6 +116,10 @@ class MyTCServer : public isobus::TaskControllerServer void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + /// @brief Sends GNSS quality (DDI 514) to every client that declares that DDI. + /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated + void send_gnss_quality(std::uint8_t quality); + 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 806746c..9eb71b1 100644 --- a/readme.md +++ b/readme.md @@ -91,6 +91,12 @@ The application logs the detected VT version, screen size, softkey dimensions, a - **Maximum Sections:** 64 (supports both individual sections and zone-based control) - **Section Control:** Generation 1 (TC-SC) with support for DDI 160/161/290 +### Guidance data sent to implements + +Sent to any implement whose device description declares the DDI as settable. + +- **GNSS quality (DDI 514):** AgOpenGPS's GPS fix quality, every 250 ms. + ## Contributing Before committing it's better to run these commands: (requires the LLVM project to be installed) diff --git a/src/app.cpp b/src/app.cpp index be27cb6..93e7cbb 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -451,6 +451,39 @@ void Application::setup_udp_connections() static std::uint32_t lastXteTransmit = 0; auto packetHandler = [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + // PGN 0xD6 (214) — GPS/IMU data from AOG (src=0x7C, frame [0x80,0x81,0x7C,0xD6,...]). + // Only the fix-quality byte is consumed today; the rest of the frame (position, + // heading, speed, etc.) is not yet parsed by this TC. + static constexpr std::size_t GNSS_FIX_QUALITY_OFFSET = 38; // frame byte 43 + static constexpr std::size_t MIN_0xD6_PAYLOAD_SIZE = GNSS_FIX_QUALITY_OFFSET + 1; + static constexpr std::uint8_t GNSS_QUALITY_MAX = 8; // SimulateMode + if (src == 0x7C && pgn == 0xD6) + { + static std::uint8_t lastLoggedQuality = 0xFF; + if (data.size() < MIN_0xD6_PAYLOAD_SIZE) + { + std::cout << "[" << get_timestamp() << "] [AOG] PGN 0xD6 received but too short for fix quality (len=" + << data.size() << ")" << std::endl; + return; + } + + std::uint8_t quality = data[GNSS_FIX_QUALITY_OFFSET]; + // AOG fix values follow the NMEA 2000 GNSS Method enumeration that DDI 514 uses: 0=invalid, 1=GPS, + // 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated. Forward 0-8 unchanged so + // the implement sees the real source; anything above 8 is not a defined value, so map it to the + // weakest real fix (1=GNSS) rather than 0=No GPS, which would falsely claim there is no position. + gnssFixQuality = (quality <= GNSS_QUALITY_MAX) ? quality : 1; + lastGnssQualityMs = isobus::SystemTiming::get_timestamp_ms(); + + if (quality != lastLoggedQuality) + { + std::cout << "[" << get_timestamp() << "] [GNSS] fix quality byte=" << static_cast(quality) + << " -> DDI514=" << static_cast(gnssFixQuality) << std::endl; + lastLoggedQuality = quality; + } + return; + } + if (src != 0x7F) { return; @@ -719,6 +752,21 @@ bool Application::update() } } + // Send GNSS quality (DDI 514) to implements every 250 ms + static std::uint32_t lastGnssSendMs = 0; + if (tcServer && isobus::SystemTiming::time_expired_ms(lastGnssSendMs, 250)) + { + // PGN 0xD6 is an independently-timed stream — treat the fix quality as unknown once it + // goes stale. Fall back to 1 (weakest real GNSS fix), not 0 (No GNSS): some implements gate + // TRACK/section control on GNSS quality being non-zero, and AOG doesn't always send 0xD6 at + // all (e.g. in Simulator mode) — 0 would falsely claim there is no position fix at all and + // can get commands rejected. + const bool gnssQualityFresh = (lastGnssQualityMs != 0) && + !isobus::SystemTiming::time_expired_ms(lastGnssQualityMs, GNSS_QUALITY_TIMEOUT_MS); + tcServer->send_gnss_quality(gnssQualityFresh ? gnssFixQuality : 1); + lastGnssSendMs = isobus::SystemTiming::get_timestamp_ms(); + } + return true; } diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 660da73..0bf80f0 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -34,6 +34,37 @@ static std::string sanitize_filename(const std::string &input) return result; } +// Finds the number of the first device element that lists the process data object as a child. +static bool find_owning_element_number(isobus::DeviceDescriptorObjectPool &pool, + const isobus::task_controller_object::DeviceProcessDataObject &processData, + std::uint16_t &elementNumber) +{ + for (std::uint32_t i = 0; i < pool.size(); i++) + { + auto object = pool.get_object_by_index(i); + if (!object || object->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceElement) + { + continue; + } + + auto elementObject = std::dynamic_pointer_cast(object); + if (!elementObject) + { + continue; + } + + for (std::uint16_t childId : elementObject->get_child_object_ids()) + { + if (childId == processData.get_object_id()) + { + elementNumber = elementObject->get_element_number(); + return true; + } + } + } + return false; +} + void ClientState::set_number_of_sections(std::uint8_t number) { numberOfSections = number; @@ -812,6 +843,37 @@ void MyTCServer::request_measurement_commands() } } + // Map GNSS quality (DDI 514) so send_gnss_quality() knows which element to address. + // Mapping only: this is a value the TC pushes to the implement, not one to subscribe to. + 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 || processDataObject->get_ddi() != static_cast(isobus::DataDescriptionIndex::GNSSQuality)) + { + continue; + } + + if (!processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) + { + std::cout << "DDI " << processDataObject->get_ddi() << " (GNSS Quality) is declared but not settable, so it is not sent" << std::endl; + continue; + } + + std::uint16_t elementNumber = 0; + if (find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber)) + { + client.second.set_element_number_for_ddi(isobus::DataDescriptionIndex::GNSSQuality, elementNumber); + std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (GNSS Quality) to element " + << elementNumber << std::endl; + } + } + std::cout << "[" << get_timestamp() << "] Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); } @@ -884,6 +946,19 @@ void MyTCServer::update_section_control_enabled(bool enabled) } } +void MyTCServer::send_gnss_quality(std::uint8_t quality) +{ + std::lock_guard lock(clientsMutex); + constexpr auto DDI = isobus::DataDescriptionIndex::GNSSQuality; + for (auto &client : clients) + { + if (client.second.has_element_number_for_ddi(DDI)) + { + send_set_value(client.first, static_cast(DDI), client.second.get_element_number_for_ddi(DDI), static_cast(quality)); + } + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex); From 46fe4250907bf2d84142793286e26b6f3aed725e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 19 Sep 2026 23:56:20 +0200 Subject: [PATCH 02/22] Announce guidance track data (DDI 507-513) to implements Parses AOG's guidance track context (PGN 0xF4) and field name (PGN 0xF3) and pushes the current track to every client whose DDOP declares the DDIs, every 250 ms while AOG has a valid track: - 507 GuidanceTrackSequenceNumber: advances whenever the track number or the reference line changes (a line switch can land on the same track index). - 508 UniqueGuidanceReferenceLineID: AOG's 16-bit line ID in the low 16 bits and a persistent per-field index in the high 16 bits, so it is unique across fields. The index comes from FieldRegistry (field_registry.csv next to settings.json). - 509-511 actual/right/left track number, with the +1 offset field testing showed implements need to stay in phase with AOG's on/off state. - 512 GuidanceLineSwathWidth: AOG's track spacing (tool width minus overlap), read from the optional bytes 10-11 of PGN 0xF4. Not sent when AOG omits it or reports 0, so older AOG builds keep working. - 513 GuidanceLineDeviation from AOG's XTE. The DDIs are mapped in request_measurement_commands() by extending the loop that maps GNSS quality, so, as for DDI 514, only DDIs an implement declares as settable are mapped. Nothing is gated on track-control negotiation: the TC pushes these values and any implement declaring them receives them. Adds unit tests for the PGN 0xF4 parser (sequence freshness and wrap, offset, validity, swath width). test_guidance_track is a dependency of iop_validator so the validation workflow, which builds only that target, builds it too. Co-Authored-By: Claude Sonnet 5 --- CMakeLists.txt | 14 +++ docs/PROTOCOL.md | 46 +++++++- include/app.hpp | 19 ++++ include/field_registry.hpp | 50 +++++++++ include/guidance_track_context.hpp | 85 ++++++++++++++ include/task_controller.hpp | 15 +++ readme.md | 1 + src/app.cpp | 102 ++++++++++++++++- src/field_registry.cpp | 134 ++++++++++++++++++++++ src/guidance_track_context.cpp | 104 +++++++++++++++++ src/task_controller.cpp | 79 ++++++++++++- tools/test_guidance_track.cpp | 173 +++++++++++++++++++++++++++++ 12 files changed, 814 insertions(+), 8 deletions(-) create mode 100644 include/field_registry.hpp create mode 100644 include/guidance_track_context.hpp create mode 100644 src/field_registry.cpp create mode 100644 src/guidance_track_context.cpp create mode 100644 tools/test_guidance_track.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ad337f7..2b41aeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,6 +159,20 @@ if(AOG_TC_VALIDATE_IOP) add_test(NAME object_pool_attributes_in_range COMMAND iop_validator "${AOG_TC_IOP_SOURCE}") + # GuidanceTrackProvider (AOG PGN 0xF4 parsing) unit tests. Made a dependency + # of iop_validator so the validation workflow, which builds only that target, + # also builds this test. + add_executable( + test_guidance_track + ${CMAKE_CURRENT_LIST_DIR}/tools/test_guidance_track.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/guidance_track_context.cpp) + target_compile_features(test_guidance_track PRIVATE cxx_std_20) + set_target_properties(test_guidance_track PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_guidance_track + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + add_dependencies(iop_validator test_guidance_track) + add_test(NAME guidance_track_parsing COMMAND test_guidance_track) + # A deliberately broken pool, so the suite asserts more than "today's pool # passes". One case per message, since a shared exit code stays green as long # as any one check still fires. diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index f7bf925..8610c30 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -95,8 +95,11 @@ All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`, except `0xD6` ( | `0xC9` (201) | Subnet detection | 5 | `[0xC9, 0xC9, IP0, IP1, IP2]` | | `0xD6` (214) | GPS/IMU data | variable (≥39 used) | Only byte 38 (fix quality) is parsed today; rest of frame is currently unused. Source `0x7C`. | | `0xE5` (229) | Section states (64 sections) | 8 | Bitfield: bit `8·j + i` of byte `j` is section `(8j + i)` ON/OFF | +| `0xEF` (239) | Machine data | variable | Only used as an AOG-liveness signal; payload not parsed. | | `0xF1` (241) | Section control mode | 1 | `[mode]` where `1` = enabled, `0` = disabled | | `0xF2` (242) | Process data | 6 | `[DDI_lo, DDI_hi, val0, val1, val2, val3]` — DDI is little-endian `uint16`; value is little-endian `int32` | +| `0xF3` (243) | Field name | variable | The open field's name as raw UTF-8; empty = field closed. | +| `0xF4` (244) | Guidance track context | 10 or 12 | Real-time AB-line/track guidance state, announced to implements as DDI 507-513 — see §5.4.1. | #### `0xC9` — Subnet detection @@ -126,10 +129,34 @@ Wraps a single ISO 11783 DDI/value pair. The TC currently dispatches on these DD |---|---|---| | `156` | Actual speed (mm/s) | Stored. If TECU enabled, broadcast as Ground/Wheel/Machine-selected speed (PGN 65256) + NMEA2000 SOG. Drives forward/reverse direction. Also produces J1939 PGN 65256 every 100 ms. | | `597` | Total distance (mm) | Stored and displayed on the VT Status page. If TECU is enabled, also populated into Speed Messages distance fields. | -| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. | +| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. Also announced to implements as DDI 513 (GuidanceLineDeviation) — see §5.4.1. | Unknown DDIs are silently ignored (PGN 0xF2 is the generic process-data channel — the TC will gain more DDIs over time). +#### `0xF3` — Field name + +The whole payload is the open field's name as raw UTF-8 — no length prefix, no offset — up to 248 bytes (longer names are truncated). An empty payload means the field was closed, which also invalidates the current track context. + +The TC maps each field name to a persistent 16-bit index, stored one `index,name` line per field in `field_registry.csv` next to `settings.json`. The index is folded into the upper 16 bits of DDI 508 (see `0xF4`), so a track's reference line ID stays unique across fields. + +#### `0xF4` — Guidance track context + +AOG's real-time AB-line/track guidance state. 12-byte payload (a 10-byte payload without the last field is also accepted): + +``` + Byte 0 Sequence counter (0–255, wraps) + Byte 1 Flags: bit0=valid, bit1=heading same way, bit2=curve mode + Bytes 2-3 Guidance Reference Line ID (uint16 LE) — 0 = no active track + Bytes 4-5 Actual Track Number (int16 LE, signed — can be negative and jump by more than 1) + Bytes 6-7 Track Number Left (int16 LE, signed) + Bytes 8-9 Track Number Right (int16 LE, signed) + Bytes 10-11 Swath Width in mm (uint16 LE) — distance between adjacent tracks; 0 = not reported +``` + +AOG sends this **only when the guidance state actually changes** — there is no heartbeat. The TC rejects any packet whose sequence number isn't strictly ahead of the last accepted one (catches duplicates, freezes, and reordered/stale UDP delivery). + +**Track-number offset:** the TC adds `+1` to all three track numbers (current/left/right) before announcing them — confirmed by field testing, not documented anywhere on AOG's side. As sent raw by AOG, a tramline implement's own on-board phase (which pass of N is "on") was consistently one pass out of sync with AOG's own intended on/off state, for both left and right passes; a uniform `+1` (independent of sign) brought them into agreement. See `GuidanceTrackProvider::parse()` (`AOG_TRACK_NUMBER_OFFSET`). + ### 2.6 PGNs outbound (TC → client) All PGNs sent **by the TC to AgIO/AgValonia** use source `0x80`. @@ -261,6 +288,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `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. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. @@ -281,6 +309,22 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co | Max sections | 64 | | Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states), plus speed/distance/guidance DDIs from the tractor side | +#### 5.4.1 Guidance data sent to implements + +The TC pushes guidance data to each client whose DDOP declares the DDI as settable. Clients can't request values from the TC, so a DDI a client doesn't declare, or declares as not settable, is never sent to it. Only clients with sections (implements, not tractors) are considered, as with the other DDI mappings. + +| DDI | Name | Sent when | +|---|---|---| +| 507 | GuidanceTrackSequenceNumber | Valid track. Increments whenever the actual track number or the reference line ID changes — not on section-control toggles. | +| 508 | UniqueGuidanceReferenceLineID | Valid track. AOG's 16-bit reference line ID in the low 16 bits, the persistent field index (see `0xF3`) in the high 16 bits. | +| 509 | ActualGuidanceTrackNumber | Valid track. Signed; can be negative and can jump by more than 1 in a single update (e.g. skipping several tracks on a headland turn). | +| 510 / 511 | GuidanceTrackNumberToTheRight / ...ToTheLeft | Valid track. | +| 512 | GuidanceLineSwathWidth | Valid track, and AOG reported a non-zero swath width in `0xF4`. AOG's track spacing (tool width minus overlap), the same spacing the track numbers are derived from. | +| 513 | GuidanceLineDeviation | Valid track. AOG's XTE in mm (PGN `0xF2`, see §2.5). | +| 514 | GNSSQuality | Always — see `0xD6`. | + +"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.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/app.hpp b/include/app.hpp index 8bc4ce5..274c1f2 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -22,6 +22,8 @@ #include "isobus/isobus/isobus_virtual_terminal_client_update_helper.hpp" #include "isobus/isobus/nmea2000_message_interface.hpp" +#include "field_registry.hpp" +#include "guidance_track_context.hpp" #include "logging_utils.hpp" #include "settings.hpp" #include "task_controller.hpp" @@ -76,6 +78,8 @@ class Application static constexpr std::uint8_t HW_MSG_ALERT = 0; static constexpr std::uint8_t HW_MSG_INFO = 1; + bool is_aog_connected() const; + std::shared_ptr settings = std::make_shared(); boost::asio::io_context ioContext = boost::asio::io_context(); std::shared_ptr udpConnections = std::make_shared(settings, ioContext); @@ -104,6 +108,21 @@ class Application std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated std::uint32_t lastGnssQualityMs = 0; ///< Timestamp of last PGN 0xD6 fix-quality update (0 = never received) static constexpr std::uint32_t GNSS_QUALITY_TIMEOUT_MS = 2000; ///< No PGN 0xD6 for this long = fix quality unknown + static constexpr std::uint32_t AOG_CONNECTION_TIMEOUT_MS = 3000; ///< No AOG packet for this long = disconnected + + // Guidance track context — real data from AOG PGN 0xF4. + GuidanceTrackProvider trackProvider; + GuidanceTrackContext currentTrackContext; + bool aogWasConnectedForTrack = false; ///< Edge-detection for AOG connect/disconnect transitions + + // Field identity — from AOG PGN 0xF3. Folded into the upper 16 bits of DDI 508 + // (see the PGN 0xF4 handling in setup_udp_connections()) so a track's guidance reference + // line ID is unique across fields, not just within whichever field AOG currently has open. + FieldRegistry fieldRegistry; + std::string currentFieldName; ///< Empty when no field is open + std::uint16_t currentFieldIndex = 0; + bool hasActiveField = false; + std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; std::uint32_t vtDisconnectedSinceMs = 0; diff --git a/include/field_registry.hpp b/include/field_registry.hpp new file mode 100644 index 0000000..13f3720 --- /dev/null +++ b/include/field_registry.hpp @@ -0,0 +1,50 @@ +/** + * @file field_registry.hpp + * @brief Persistent field-name -> field-index mapping, used to make ISOBUS TRACK's + * DDI 508 (Unique Guidance Reference Line ID) actually unique across fields. + * + * AOG's own PGN 0xF4 guidance reference ID is only a 16-bit value scoped to whatever + * field is currently open in AOG - it is not guaranteed unique across different fields. + * An implement that caches per-track state (e.g. an offset) keyed on DDI 508 alone can + * therefore collide across a field switch. This registry assigns each field name a + * stable index, which the caller folds into the upper 16 bits of the 32-bit DDI 508 + * value (see Application's PGN 0xF3/0xF4 handling), leaving AOG's own 16-bit ID in the + * lower 16 bits untouched. + */ + +#pragma once + +#include +#include +#include + +/// @brief Loads/persists a field-name -> field-index mapping from a plain text file. +/// +/// Deliberately its own file rather than part of settings.json: the mapping only grows +/// over time (one line per field ever opened), so a user may want to wipe it on its own +/// (e.g. to reclaim indices) without touching the rest of their configuration. +class FieldRegistry +{ +public: + /// @brief Loads the registry from disk, if present. A missing or unreadable file + /// just starts empty - fields get freshly (re-)indexed and persisted as they're seen. + FieldRegistry(); + + /// @brief Returns the persistent index for a field name, assigning and persisting a + /// new one the first time this name is seen. + /// @param fieldName UTF-8 field folder name, as received from AOG PGN 0xF3. + /// @returns A stable index. Once the 16-bit space is exhausted (65536 distinct + /// field names - far beyond realistic use), the most recently assigned index is + /// reused and a warning is logged, rather than silently colliding with an existing + /// field. + std::uint16_t get_or_assign_index(const std::string &fieldName); + +private: + void load(); + void append_entry(const std::string &fieldName, std::uint16_t index); + + std::string filePath; + std::unordered_map nameToIndex; + std::uint16_t nextIndex = 0; + bool nextIndexExhausted = false; +}; diff --git a/include/guidance_track_context.hpp b/include/guidance_track_context.hpp new file mode 100644 index 0000000..6240e16 --- /dev/null +++ b/include/guidance_track_context.hpp @@ -0,0 +1,85 @@ +/** + * @file guidance_track_context.hpp + * @brief Abstraction layer between AOG input and ISOBUS TRACK (Generation 1) protocol. + * + * This header defines the GuidanceTrackContext struct and the GuidanceTrackProvider + * that consumes AOG PGN 0xF4 guidance-track data. + * + * The ISOBUS TRACK sender consumes a GuidanceTrackContext without caring how it + * was produced. The GuidanceTrackContext maps directly to ISOBUS DDIs 508–511. + */ + +#pragma once + +#include +#include +#include + +/** + * @brief Immutable snapshot of guidance-track state consumed by the ISOBUS TRACK sender. + * + * Corresponds to ISOBUS DDIs: + * - guidanceReferenceLineId -> DDI 508 (Unique Guidance Reference Line ID) + * - actualTrackNumber -> DDI 509 (Actual Guidance Track Number) + * - trackNumberRight -> DDI 510 (Guidance Track Number to the Right) + * - trackNumberLeft -> DDI 511 (Guidance Track Number to the Left) + * - swathWidthMm -> DDI 512 (Guidance Line Swath Width) + */ +struct GuidanceTrackContext +{ + std::uint32_t guidanceReferenceLineId = 1; ///< DDI 508 + std::int32_t actualTrackNumber = 0; ///< DDI 509 — signed; track 0 is valid + std::int32_t trackNumberRight = -1; ///< DDI 510 + std::int32_t trackNumberLeft = 1; ///< DDI 511 + std::uint32_t swathWidthMm = 0; ///< DDI 512 — distance between adjacent tracks; 0 = not reported by AOG + bool valid = false; ///< Context has been initialized with at least one update +}; + +/** + * @brief Real guidance-track provider that consumes AOG PGN 0xF4 (244) data. + * + * PGN 0xF4 payload layout (10 data bytes, optionally 12): + * Byte 0: Sequence counter (0–255, wrapping) + * Byte 1: Flags (bit 0 = valid, bit 1 = heading same way, bit 2 = curve mode) + * Bytes 2-3: Guidance Reference ID (uint16 LE) + * Bytes 4-5: Current Track Number (int16 LE, signed) + * Bytes 6-7: Track Number Left (int16 LE, signed) + * Bytes 8-9: Track Number Right (int16 LE, signed) + * Bytes 10-11: Swath Width in mm (uint16 LE), optional — only present from newer AOG builds + * + * Track numbers (current/left/right) are each shifted by +1 from the raw wire value + * (see AOG_TRACK_NUMBER_OFFSET in parse()) to match the tram-pattern phase implements + * expect — confirmed by field testing, not part of AOG's own documented wire format. + * + * Sequence tracking: + * - Rejects any packet whose sequence number is not strictly ahead of the last + * accepted one (signed delta over the 0-255 wrap), catching both frozen/duplicate + * data and reordered/stale UDP packets arriving out of order. + * - Call reset() after a disconnect to treat the next packet as a fresh start. + * + * Returns valid=true when data is valid, refId != 0, and sequence is fresh. + */ +class GuidanceTrackProvider +{ +public: + /// Minimum payload size (10 data bytes) + static constexpr std::size_t MIN_PAYLOAD_SIZE = 10; + + /// Payload size that includes the swath width + static constexpr std::size_t PAYLOAD_SIZE_WITH_SWATH_WIDTH = 12; + + /** + * @brief Parse AOG PGN 0xF4 payload and produce a GuidanceTrackContext. + * + * @param data Payload bytes (after UDP header stripping) + * @return GuidanceTrackContext with valid=true if parse succeeded and flags indicate valid data + */ + GuidanceTrackContext parse(std::span data); + + /// @brief Reset sequence tracking (e.g., after AOG disconnect timeout). + /// The next parse() call is treated as a fresh start — no delta comparison. + void reset(); + +private: + std::optional lastSequence_; ///< Unset until the first packet is accepted +}; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 839a4fc..517f17a 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -9,6 +9,7 @@ #pragma once +#include "guidance_track_context.hpp" #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" @@ -63,6 +64,11 @@ class ClientState void set_element_work_state(std::uint16_t elementNumber, bool isWorking); bool try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const; + /// @brief Advances the DDI 507 sequence number if the track or reference line differs from the + /// last one announced to this client, and remembers the new values. + /// @returns The sequence number to announce. + std::uint32_t update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId); + 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 @@ -79,6 +85,9 @@ class ClientState bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting bool usesPerElementControl = false; ///< Legacy mode: use per-element setpoint instead of condensed std::uint16_t perElementSetpointDDI = 0; ///< The DDI to use for per-element setpoints (289 or 141), 0 if not applicable + 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 }; // Create the task controller server object, this will handle all the ISOBUS communication for us @@ -120,6 +129,12 @@ class MyTCServer : public isobus::TaskControllerServer /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated void send_gnss_quality(std::uint8_t quality); + /// @brief Announces the current guidance track (DDI 507-511) and line deviation (DDI 513) to every + /// client that declares those DDIs. Does nothing while the context is not valid. + /// @param ctx Current guidance track state from AOG + /// @param lineDeviationMm Deviation from the guidance line in mm + void send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm); + 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 9eb71b1..0af81c3 100644 --- a/readme.md +++ b/readme.md @@ -96,6 +96,7 @@ The application logs the detected VT version, screen size, softkey dimensions, a Sent to any implement whose device description declares the DDI as settable. - **GNSS quality (DDI 514):** AgOpenGPS's GPS fix quality, every 250 ms. +- **Guidance track (DDI 507-513):** the current track number, the tracks to its left and right, the track spacing, a reference line ID that is unique across fields, and the deviation from the guidance line, every 250 ms while AgOpenGPS has an active track. ## Contributing diff --git a/src/app.cpp b/src/app.cpp index 93e7cbb..675f88c 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -509,6 +509,82 @@ void Application::setup_udp_connections() log() << "Received request from AOG to change section control state to " << (sectionControlState == 1 ? "enabled" : "disabled") << std::endl; tcServer->update_section_control_enabled(sectionControlState == 1); } + else if (pgn == 0xEF) // 239 - Machine Data + { + // Not parsed; only counts as AOG liveness. + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + } + else if (pgn == 0xF3) // 243 - Field Name + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // The whole payload IS the UTF-8 field name — no length prefix, no offset. + // Confirmed against a live packet: a documented "length byte at offset 4, + // name at offset 5+" layout does not match what AOG actually sends — the + // payload was exactly N raw UTF-8 name bytes, nothing else. An empty + // payload means the field is closed. + if (data.empty()) + { + if (hasActiveField) + { + log("Field") << "Field closed: " << currentFieldName << std::endl; + } + currentFieldName.clear(); + hasActiveField = false; + // Invalidate any in-flight track context immediately — broadcasting + // DDI 508 without a field to scope it to would defeat the point of the + // field index folded into it below. + currentTrackContext.valid = false; + } + else + { + constexpr std::size_t MAX_FIELD_NAME_BYTES = 248; + std::size_t nameLength = data.size(); + if (nameLength > MAX_FIELD_NAME_BYTES) + { + log("Field") << "PGN 0xF3 name of " << nameLength << " bytes exceeds the documented " + << MAX_FIELD_NAME_BYTES << "-byte max; truncating." << std::endl; + nameLength = MAX_FIELD_NAME_BYTES; + } + + std::string fieldName(reinterpret_cast(data.data()), nameLength); + if (fieldName != currentFieldName || !hasActiveField) + { + currentFieldName = fieldName; + currentFieldIndex = fieldRegistry.get_or_assign_index(fieldName); + hasActiveField = true; + log("Field") << "Field opened: " << currentFieldName << " (index " << currentFieldIndex << ")" << std::endl; + } + } + } + else if (pgn == 0xF4) // 244 - Guidance Track Context + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // Always update currentTrackContext: when AOG sends valid=false + // (guidance off / no active track), the context must be invalidated + // so the TC stops broadcasting stale track data. + // Note: parse() handles short-payload validation internally. + currentTrackContext = trackProvider.parse(data); + + // AOG's own guidance reference ID (see GuidanceTrackProvider) is only unique + // within whichever field AOG currently has open — fold in the field's own + // persistent index (upper 16 bits) so DDI 508 is unique across fields too. + // Without an active field, there's nothing to scope the ID to — don't send it. + if (currentTrackContext.valid) + { + if (hasActiveField) + { + currentTrackContext.guidanceReferenceLineId = + (static_cast(currentFieldIndex) << 16) | + (currentTrackContext.guidanceReferenceLineId & 0xFFFFu); + } + else + { + currentTrackContext.valid = false; + } + } + } else if (pgn == 0xF2 && data.size() >= 6) // Process Data { lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); @@ -767,9 +843,33 @@ bool Application::update() lastGnssSendMs = isobus::SystemTiming::get_timestamp_ms(); } + // Send guidance track data (DDI 507-511, 513) to implements every 250 ms + static std::uint32_t lastTrackSendMs = 0; + if (tcServer && isobus::SystemTiming::time_expired_ms(lastTrackSendMs, 250)) + { + // AOG only sends PGN 0xF4 when the guidance track actually changes (no heartbeat) — + // long gaps between packets are the normal state while driving straight, not staleness. + // Only clear the context on a real AOG disconnect (edge-triggered, not every tick). + const bool aogConnectedNow = is_aog_connected(); + if (!aogConnectedNow && aogWasConnectedForTrack) + { + currentTrackContext.valid = false; + trackProvider.reset(); // Treat next packet as fresh start after the gap + } + aogWasConnectedForTrack = aogConnectedNow; + + tcServer->send_guidance_track_data(currentTrackContext, lastXteValue); + lastTrackSendMs = isobus::SystemTiming::get_timestamp_ms(); + } + return true; } +bool Application::is_aog_connected() const +{ + return (lastAogPacketMs != 0) && !isobus::SystemTiming::time_expired_ms(lastAogPacketMs, AOG_CONNECTION_TIMEOUT_MS); +} + void Application::send_hardware_message(const std::string &text, std::uint8_t duration, std::uint8_t color) { if (!text.empty()) @@ -1209,7 +1309,7 @@ void Application::update_vt_client() sync_vt_config_once(); - const bool aogConnected = (lastAogPacketMs != 0) && !isobus::SystemTiming::time_expired_ms(lastAogPacketMs, 3000); + const bool aogConnected = is_aog_connected(); vtUpdateHelper->set_numeric_value(VTSpeedValue, aogConnected ? static_cast(std::abs(lastSpeedValue)) : 0U); vtUpdateHelper->set_numeric_value(VTXteValue, aogConnected ? (static_cast(lastXteValue) ^ 0x80000000U) : 0x80000000U); diff --git a/src/field_registry.cpp b/src/field_registry.cpp new file mode 100644 index 0000000..16958e1 --- /dev/null +++ b/src/field_registry.cpp @@ -0,0 +1,134 @@ +#include "field_registry.hpp" + +#include "logging_utils.hpp" +#include "settings.hpp" + +#include +#include +#include + +namespace +{ + constexpr char REGISTRY_FILE_NAME[] = "field_registry.csv"; + + // Field names come from UDP input and are persisted one-per-line as + // "index,name". A CR or LF embedded in the name would split that into + // multiple lines and corrupt the registry on the next load(), so strip + // them before the name is used as a map key, persisted, or logged. + std::string strip_crlf(const std::string &name) + { + std::string sanitized; + sanitized.reserve(name.size()); + for (char c : name) + { + if (c != '\r' && c != '\n') + { + sanitized.push_back(c); + } + } + return sanitized; + } +} + +FieldRegistry::FieldRegistry() +{ + filePath = Settings::get_filename_path(REGISTRY_FILE_NAME); + load(); +} + +void FieldRegistry::load() +{ + std::ifstream in(filePath); + if (!in.is_open()) + { + return; // No registry yet - fields will be indexed fresh as they're seen. + } + + std::string line; + std::uint32_t highestIndex = 0; + bool haveAny = false; + while (std::getline(in, line)) + { + if (!line.empty() && line.back() == '\r') + { + line.pop_back(); + } + const auto commaPos = line.find(','); + if (commaPos == std::string::npos || commaPos == 0) + { + continue; // Malformed line - skip it rather than aborting the whole load. + } + + try + { + const unsigned long parsedIndex = std::stoul(line.substr(0, commaPos)); + if (parsedIndex > std::numeric_limits::max()) + { + continue; + } + const auto index = static_cast(parsedIndex); + const std::string name = line.substr(commaPos + 1); + nameToIndex[name] = index; + haveAny = true; + if (static_cast(index) > highestIndex) + { + highestIndex = index; + } + } + catch (const std::exception &) + { + continue; // Non-numeric index - skip this line. + } + } + + if (haveAny) + { + nextIndex = (highestIndex < std::numeric_limits::max()) ? static_cast(highestIndex + 1) : std::numeric_limits::max(); + nextIndexExhausted = (highestIndex >= std::numeric_limits::max()); + } + + std::cout << "[" << get_timestamp() << "] [FieldRegistry] Loaded " << nameToIndex.size() << " field(s) from " << filePath << std::endl; +} + +void FieldRegistry::append_entry(const std::string &fieldName, std::uint16_t index) +{ + std::ofstream out(filePath, std::ios::app); + if (!out.is_open()) + { + std::cout << "[" << get_timestamp() << "] [FieldRegistry] Failed to persist field '" << fieldName << "' (could not open " << filePath << " for append)" << std::endl; + return; + } + out << index << ',' << fieldName << '\n'; +} + +std::uint16_t FieldRegistry::get_or_assign_index(const std::string &rawFieldName) +{ + const std::string fieldName = strip_crlf(rawFieldName); + auto it = nameToIndex.find(fieldName); + if (it != nameToIndex.end()) + { + return it->second; + } + + if (nextIndexExhausted) + { + std::cout << "[" << get_timestamp() << "] [FieldRegistry] Field index space exhausted (65536 fields already registered); " + << "reusing the last index for '" << fieldName << "' instead of assigning a new one." << std::endl; + return nextIndex; + } + + const std::uint16_t assigned = nextIndex; + nameToIndex[fieldName] = assigned; + if (assigned == std::numeric_limits::max()) + { + nextIndexExhausted = true; + } + else + { + ++nextIndex; + } + + append_entry(fieldName, assigned); + std::cout << "[" << get_timestamp() << "] [FieldRegistry] Assigned index " << assigned << " to field '" << fieldName << "'" << std::endl; + return assigned; +} diff --git a/src/guidance_track_context.cpp b/src/guidance_track_context.cpp new file mode 100644 index 0000000..34d3184 --- /dev/null +++ b/src/guidance_track_context.cpp @@ -0,0 +1,104 @@ +/** + * @file guidance_track_context.cpp + * @brief Implementation of GuidanceTrackProvider, see guidance_track_context.hpp. + */ + +#include "guidance_track_context.hpp" +#include "logging_utils.hpp" + +#include + +namespace +{ + /// @brief Decode a little-endian uint16 from a 2-byte span starting at offset. + std::uint16_t decode_le_u16(std::span data, std::size_t offset) + { + return static_cast(data[offset]) | + (static_cast(data[offset + 1]) << 8); + } + + /// @brief Decode a little-endian, signed int16 from a 2-byte span starting at offset. + std::int16_t decode_le_i16(std::span data, std::size_t offset) + { + return static_cast(decode_le_u16(data, offset)); + } +} // namespace + +GuidanceTrackContext GuidanceTrackProvider::parse(std::span data) +{ + GuidanceTrackContext ctx; + + if (data.size() < MIN_PAYLOAD_SIZE) + { + std::cout << "[" << get_timestamp() << "] [TRACK][real] PGN 0xF4 too short (len=" + << data.size() << ")" << std::endl; + return ctx; + } + + // Parse fields + std::uint8_t sequence = data[0]; + std::uint8_t flags = data[1]; + bool isValid = (flags & 0x01) != 0; + bool headingSameWay = (flags & 0x02) != 0; + bool curveMode = (flags & 0x04) != 0; + + std::uint16_t refId = decode_le_u16(data, 2); + + // AOG's raw track-number wire convention is one pass off from what a tramline + // implement's own on-board phase computation (e.g. "which of N passes is this") + // expects, for both left and right passes. Confirmed via field testing: with the + // raw value relayed as-is, the sprayer's ON/OFF tram state was consistently one + // pass out of phase with AOG's own intended tram state; shifting every value by + // +1 (independent of sign) brought them into agreement across every pass tested. + static constexpr std::int16_t AOG_TRACK_NUMBER_OFFSET = 1; + std::int16_t currentTrack = static_cast(decode_le_i16(data, 4) + AOG_TRACK_NUMBER_OFFSET); + std::int16_t trackLeft = static_cast(decode_le_i16(data, 6) + AOG_TRACK_NUMBER_OFFSET); + std::int16_t trackRight = static_cast(decode_le_i16(data, 8) + AOG_TRACK_NUMBER_OFFSET); + + // Sequence freshness check: signed delta over the 0-255 wrap must be strictly + // positive (forward progress). Rejects both frozen/duplicate packets (delta == 0) + // and reordered/stale packets that arrived out of order (delta < 0). + const char *outcome; + + if (lastSequence_.has_value() && static_cast(sequence - *lastSequence_) <= 0) + { + outcome = "REJECTED (stale/duplicate/out-of-order sequence)"; + } + else + { + lastSequence_ = sequence; + + if (!isValid || refId == 0) + { + outcome = "guidance OFF (no active track)"; + } + else + { + ctx.guidanceReferenceLineId = refId; + ctx.actualTrackNumber = currentTrack; + ctx.trackNumberLeft = trackLeft; + ctx.trackNumberRight = trackRight; + if (data.size() >= PAYLOAD_SIZE_WITH_SWATH_WIDTH) + { + ctx.swathWidthMm = decode_le_u16(data, 10); + } + ctx.valid = true; + outcome = "ACCEPTED"; + } + } + + std::cout << "[" << get_timestamp() << "] [TRACK][real] seq=" << static_cast(sequence) + << " flags=0x" << std::hex << static_cast(flags) << std::dec + << " (valid=" << isValid << " sameHeading=" << headingSameWay << " curve=" << curveMode << ")" + << " ref=" << refId + << " left=" << trackLeft << " actual=" << currentTrack << " right=" << trackRight + << " swath=" << ctx.swathWidthMm << "mm" + << " -> " << outcome << std::endl; + + return ctx; +} + +void GuidanceTrackProvider::reset() +{ + lastSequence_.reset(); +} diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 0bf80f0..8f9ca42 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -65,6 +65,25 @@ static bool find_owning_element_number(isobus::DeviceDescriptorObjectPool &pool, return false; } +// The DDIs the TC pushes to implements as guidance data. +static bool is_guidance_data_ddi(std::uint16_t ddi) +{ + switch (static_cast(ddi)) + { + case isobus::DataDescriptionIndex::GNSSQuality: + case isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber: + case isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID: + case isobus::DataDescriptionIndex::ActualGuidanceTrackNumber: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft: + case isobus::DataDescriptionIndex::GuidanceLineSwathWidth: + case isobus::DataDescriptionIndex::GuidanceLineDeviation: + return true; + default: + return false; + } +} + void ClientState::set_number_of_sections(std::uint8_t number) { numberOfSections = number; @@ -355,6 +374,18 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool & return false; } +std::uint32_t ClientState::update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId) +{ + // A line switch can land on the same track index, so compare the reference line too. + if ((trackNumber != lastSentTrackNumber) || (referenceLineId != lastSentReferenceLineId)) + { + guidanceTrackSequenceNumber++; + lastSentTrackNumber = trackNumber; + lastSentReferenceLineId = referenceLineId; + } + return guidanceTrackSequenceNumber; +} + MyTCServer::MyTCServer(std::shared_ptr internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -843,8 +874,8 @@ void MyTCServer::request_measurement_commands() } } - // Map GNSS quality (DDI 514) so send_gnss_quality() knows which element to address. - // Mapping only: this is a value the TC pushes to the implement, not one to subscribe to. + // Map the guidance DDIs so the send_* methods know which element to address. + // Mapping only: these are values the TC pushes to the implement, not ones to subscribe to. for (std::uint32_t i = 0; i < client.second.get_pool().size(); i++) { auto object = client.second.get_pool().get_object_by_index(i); @@ -854,22 +885,22 @@ void MyTCServer::request_measurement_commands() } auto processDataObject = std::dynamic_pointer_cast(object); - if (!processDataObject || processDataObject->get_ddi() != static_cast(isobus::DataDescriptionIndex::GNSSQuality)) + if (!processDataObject || !is_guidance_data_ddi(processDataObject->get_ddi())) { continue; } if (!processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) { - std::cout << "DDI " << processDataObject->get_ddi() << " (GNSS Quality) is declared but not settable, so it is not sent" << std::endl; + std::cout << "DDI " << processDataObject->get_ddi() << " (" << isobus::DataDictionary::get_entry(processDataObject->get_ddi()).to_string() << ") is declared but not settable, so it is not sent" << std::endl; continue; } std::uint16_t elementNumber = 0; if (find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber)) { - client.second.set_element_number_for_ddi(isobus::DataDescriptionIndex::GNSSQuality, elementNumber); - std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (GNSS Quality) to element " + client.second.set_element_number_for_ddi(static_cast(processDataObject->get_ddi()), elementNumber); + std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (" << isobus::DataDictionary::get_entry(processDataObject->get_ddi()).to_string() << ") to element " << elementNumber << std::endl; } } @@ -959,6 +990,42 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality) } } +void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm) +{ + if (!ctx.valid) + { + return; + } + + std::lock_guard lock(clientsMutex); + for (auto &client : clients) + { + auto &state = client.second; + + auto trySend = [&](isobus::DataDescriptionIndex ddi, std::int32_t value) { + if (state.has_element_number_for_ddi(ddi)) + { + send_set_value(client.first, static_cast(ddi), state.get_element_number_for_ddi(ddi), value); + } + }; + + const std::uint32_t sequenceNumber = state.update_guidance_track_sequence(ctx.actualTrackNumber, ctx.guidanceReferenceLineId); + + // Coherent ordering per the TRACK guideline: + // 507 (sequence) -> 508 (ref line ID) -> 509 (actual track) -> 510 (right) -> 511 (left) + trySend(isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber, static_cast(sequenceNumber)); + trySend(isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID, static_cast(ctx.guidanceReferenceLineId)); + trySend(isobus::DataDescriptionIndex::ActualGuidanceTrackNumber, ctx.actualTrackNumber); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight, ctx.trackNumberRight); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft, ctx.trackNumberLeft); + if (ctx.swathWidthMm != 0) + { + trySend(isobus::DataDescriptionIndex::GuidanceLineSwathWidth, static_cast(ctx.swathWidthMm)); + } + trySend(isobus::DataDescriptionIndex::GuidanceLineDeviation, lineDeviationMm); + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex); diff --git a/tools/test_guidance_track.cpp b/tools/test_guidance_track.cpp new file mode 100644 index 0000000..161dceb --- /dev/null +++ b/tools/test_guidance_track.cpp @@ -0,0 +1,173 @@ +/** + * @brief Unit tests for GuidanceTrackProvider, which parses AOG's PGN 0xF4. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "guidance_track_context.hpp" + +#include +#include +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +static std::array make_packet(std::uint8_t sequence, + std::uint8_t flags, + std::uint16_t referenceId, + std::int16_t current, + std::int16_t left, + std::int16_t right) +{ + auto lo = [](std::int16_t v) { return static_cast(static_cast(v) & 0xFF); }; + auto hi = [](std::int16_t v) { return static_cast((static_cast(v) >> 8) & 0xFF); }; + return { sequence, + flags, + static_cast(referenceId & 0xFF), + static_cast(referenceId >> 8), + lo(current), + hi(current), + lo(left), + hi(left), + lo(right), + hi(right) }; +} + +static GuidanceTrackContext parse(GuidanceTrackProvider &provider, const std::array &packet) +{ + return provider.parse(std::span(packet.data(), packet.size())); +} + +static GuidanceTrackContext parse_with_swath_width(GuidanceTrackProvider &provider, std::uint16_t swathWidthMm) +{ + auto base = make_packet(0, 0x01, 1, 0, -1, 1); + std::array packet{}; + std::copy(base.begin(), base.end(), packet.begin()); + packet[10] = static_cast(swathWidthMm & 0xFF); + packet[11] = static_cast(swathWidthMm >> 8); + return provider.parse(std::span(packet.data(), packet.size())); +} + +static void test_valid_packet_fields_and_offset() +{ + std::printf("test_valid_packet_fields_and_offset\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 0x1234, 5, 4, 6)); + + check(ctx.valid, "valid packet is accepted"); + check(ctx.guidanceReferenceLineId == 0x1234, "reference line ID is passed through"); + check(ctx.actualTrackNumber == 6, "actual track number gets the +1 offset"); + check(ctx.trackNumberLeft == 5, "left track number gets the +1 offset"); + check(ctx.trackNumberRight == 7, "right track number gets the +1 offset"); +} + +static void test_swath_width() +{ + std::printf("test_swath_width\n"); + + GuidanceTrackProvider provider; + auto ctx = parse_with_swath_width(provider, 18000); + check(ctx.valid, "12-byte packet is accepted"); + check(ctx.swathWidthMm == 18000, "swath width is decoded as little-endian mm"); + + GuidanceTrackProvider legacyProvider; + auto legacy = parse(legacyProvider, make_packet(0, 0x01, 1, 0, -1, 1)); + check(legacy.valid, "10-byte packet is still accepted"); + check(legacy.swathWidthMm == 0, "10-byte packet reports no swath width"); +} + +static void test_negative_track_numbers() +{ + std::printf("test_negative_track_numbers\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 1, -3, -4, -2)); + + check(ctx.valid, "negative track numbers are valid"); + check(ctx.actualTrackNumber == -2, "-3 + offset = -2"); + check(ctx.trackNumberLeft == -3, "-4 + offset = -3"); + check(ctx.trackNumberRight == -1, "-2 + offset = -1"); +} + +static void test_short_payload_rejected() +{ + std::printf("test_short_payload_rejected\n"); + + GuidanceTrackProvider provider; + const std::array shortPacket = { 0, 0x01, 1, 0, 0, 0, 0, 0, 0 }; + auto ctx = provider.parse(std::span(shortPacket.data(), shortPacket.size())); + + check(!ctx.valid, "a 9-byte payload is rejected"); +} + +static void test_guidance_off() +{ + std::printf("test_guidance_off\n"); + + GuidanceTrackProvider provider; + check(!parse(provider, make_packet(0, 0x00, 7, 1, 0, 2)).valid, "valid flag clear -> invalid"); + check(!parse(provider, make_packet(1, 0x01, 0, 1, 0, 2)).valid, "reference line ID 0 -> invalid"); + check(parse(provider, make_packet(2, 0x01, 7, 1, 0, 2)).valid, "a guidance-off packet still advances the sequence"); +} + +static void test_sequence_freshness() +{ + std::printf("test_sequence_freshness\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "first packet is accepted"); + check(!parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "duplicate sequence is rejected"); + check(!parse(provider, make_packet(9, 0x01, 1, 0, 0, 0)).valid, "older sequence is rejected"); + check(parse(provider, make_packet(11, 0x01, 1, 0, 0, 0)).valid, "next sequence is accepted"); +} + +static void test_sequence_wrap() +{ + std::printf("test_sequence_wrap\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 is accepted"); + check(parse(provider, make_packet(0, 0x01, 1, 0, 0, 0)).valid, "0 after 255 is forward progress"); + check(!parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 after 0 is a step back"); +} + +static void test_reset() +{ + std::printf("test_reset\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "packet accepted"); + provider.reset(); + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "same sequence is accepted again after reset"); +} + +int main() +{ + test_valid_packet_fields_and_offset(); + test_swath_width(); + test_negative_track_numbers(); + test_short_payload_rejected(); + test_guidance_off(); + test_sequence_freshness(); + test_sequence_wrap(); + test_reset(); + + if (failures != 0) + { + std::fprintf(stderr, "%d check(s) failed\n", failures); + return 1; + } + std::printf("All guidance track tests passed\n"); + return 0; +} 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 03/22] 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); From e63db02e8d82a32b23a6524a5eda7a454b453a28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 20 Sep 2026 00:15:52 +0200 Subject: [PATCH 04/22] Add Tractor Facilities (PGN 65033) for the TECU Adds TractorFacilities: builds and sends the PGN 65033 facility bitmask (ISO 11783-7 B.24.3), advertising which PGNs the TECU actually broadcasts (ground- and wheel-based speed, distance and direction). It is sent once on power-up and on PGN 65033 request. PGN 65032 (Required Tractor Facilities) requests are logged at debug level without changing the response. Also adds diagnostic logging for Request-for-Repetition-Rate (PGN 0xCC00), both the addressed and the broadcast form, and unit tests for the facilities bitmask encode/decode logic (tools/test_tractor_facilities.cpp, registered via CTest). The IOP validation workflow built only the iop_validator target and then ran all of ctest, so it now builds test_tractor_facilities as well. Co-Authored-By: Claude Sonnet 5 --- .github/workflows/validate-iop.yml | 2 +- CMakeLists.txt | 15 + docs/CONCURRENCY.md | 5 +- docs/PROTOCOL.md | 35 +++ include/app.hpp | 4 + include/tractor_facilities.hpp | 158 ++++++++++ src/app.cpp | 86 ++++++ src/tractor_facilities.cpp | 452 +++++++++++++++++++++++++++++ tools/test_tractor_facilities.cpp | 345 ++++++++++++++++++++++ 9 files changed, 1099 insertions(+), 3 deletions(-) create mode 100644 include/tractor_facilities.hpp create mode 100644 src/tractor_facilities.cpp create mode 100644 tools/test_tractor_facilities.cpp diff --git a/.github/workflows/validate-iop.yml b/.github/workflows/validate-iop.yml index 13fd36f..3a73640 100644 --- a/.github/workflows/validate-iop.yml +++ b/.github/workflows/validate-iop.yml @@ -34,7 +34,7 @@ jobs: -Wno-dev - name: Build - run: cmake --build build --config Release --target iop_validator + run: cmake --build build --config Release --target iop_validator test_tractor_facilities - name: Validate object pool run: ctest --test-dir build --output-on-failure diff --git a/CMakeLists.txt b/CMakeLists.txt index ad337f7..e1e202d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,6 +185,21 @@ if(AOG_TC_VALIDATE_IOP) add_test(NAME malformed_pool_exit_status COMMAND iop_validator "${AOG_TC_MALFORMED_IOP}") set_tests_properties(malformed_pool_exit_status PROPERTIES WILL_FAIL TRUE) + + # Tractor Facilities (PGN 65033) encode/decode unit tests + add_executable( + test_tractor_facilities + ${CMAKE_CURRENT_LIST_DIR}/tools/test_tractor_facilities.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/tractor_facilities.cpp) + target_compile_features(test_tractor_facilities PRIVATE cxx_std_20) + set_target_properties(test_tractor_facilities PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_tractor_facilities + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + target_link_libraries( + test_tractor_facilities PRIVATE isobus::Isobus isobus::HardwareIntegration + isobus::Utility) + add_test(NAME tractor_facilities_encode_decode + COMMAND test_tractor_facilities) endif() if(WIN32) diff --git a/docs/CONCURRENCY.md b/docs/CONCURRENCY.md index 8df81de..3d6cd47 100644 --- a/docs/CONCURRENCY.md +++ b/docs/CONCURRENCY.md @@ -38,8 +38,9 @@ registered: `softKeyEventDispatcher`/`buttonEventDispatcher`/`changeNumericValueEventDispatcher`/etc. synchronously). Any raw `add_global_parameter_group_number_callback`/`add_any_control_function_parameter_group_number_callback` - registered directly in `app.cpp` is in this category too, unless proven otherwise the same way: - read the registration call, not the surrounding comments. + registered directly in `app.cpp`/`tractor_facilities.cpp` (PGN-request handlers, diagnostic + loggers) is in this category too, unless proven otherwise the same way: read the registration + call, not the surrounding comments. If you're not sure which category a new callback falls into, trace it the way this file's history did: find where it's registered with `CANNetworkManager`/`ControlFunction` and check whether that diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index e9d22e8..a1f698e 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -251,6 +251,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `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 | TECU | Announces Class 1 BasicTractorECUServer (no options). | +| `0xFE09` (PGN 65033 Tractor Facilities) | Power-up + on request | TECU | 8-byte facility bitmask advertising which PGNs the TECU actually broadcasts. See §5.6. | | NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. @@ -261,6 +262,7 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co - **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. - **Process data acknowledges (PDACK)**: logged. +- **PGN 65033 requests**: answered with the Tractor Facilities response (§5.6). An implement may also send PGN 65032 (Required Tractor Facilities) to advertise what it needs; the TC logs this at debug level but does not change its response. ### 5.4 ISOBUS feature scope @@ -280,6 +282,39 @@ The pool was authored for a 480-pixel data mask and an 80-pixel softkey designat At connection, the TC logs the VT version, screen dimensions, softkey dimensions, and virtual/physical softkey counts. It warns when fewer than five virtual softkeys are available. If a VT address is detected but the client has not connected after 30 seconds, it logs the reported capabilities and recovery guidance. Clear the terminal's stored/cached object pools first when diagnosing an upload failure, because stale pools and full non-volatile pool storage can prevent an otherwise compatible upload. +### 5.6 Tractor Facilities (PGN 65033) + +When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.24.3) and broadcasts the response once on power-up. The 8-byte payload is a bitfield where each bit signals that the TECU actually transmits the corresponding PGN at its defined repetition rate. + +**Facilities advertised (bits set to 1):** + +| Byte | Bit(s) | Facility | Condition | +|---|---|---|---| +| 1 | 8,7 | TECU class | Always `00` (Class 1). | +| 1 | 2 | Ground-based speed (PGN 65097) | `speedMessagesInterface` exists (always true when TECU is enabled). | +| 1 | 3 | Wheel-based speed (PGN 65096) | `speedMessagesInterface` exists (always true when TECU is enabled). | +| 3 | 7,6 | Ground-based distance + direction | Same as ground-based speed. | +| 3 | 5,4 | Wheel-based distance + direction | Same as wheel-based speed. | + +**Facilities NOT advertised (bits always 0):** + +- Engine speed — no engine CAN access. +- Time/date (PGN 65254) — the TC does not broadcast it. +- Power management — no key switch or power timer signals. +- Hitch position / in-work / draft — the hydraulic lift output is a command we issue, not measured feedback; implements would trust it for work-state logic. +- PTO shaft speed / engagement — no PTO sensor. +- Lighting — no lighting controller. +- Language command storage (PGN 65039) — not broadcast by the TECU. +- Auxiliary valve commands / status — no valve interface. +- Selected speed (PGN 65265) — not broadcast. +- Navigation position data / high-output position — NMEA 2000 position PGNs are not forwarded over Fast Packet. +- Front hitch / PTO — no front hitch or PTO sensors. +- All reserved bits (byte 2 bits 2–1, byte 4 bits 3–1, byte 5 bit 5, byte 7, byte 8 including the reserved-bit indicator at bit 1). + +**Default payload** (TECU enabled, speed broadcasts active): `[0x06, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00]`. + +**PGN 65032 (Required Tractor Facilities):** When an implement broadcasts what it needs, the TC logs the request at debug level. The response is not modified based on the implement's requirements — a facility bit is set to 1 only when backed by a live broadcast. + --- ## 6. Example clients diff --git a/include/app.hpp b/include/app.hpp index d74de11..3a5f64b 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -25,6 +25,7 @@ #include "logging_utils.hpp" #include "settings.hpp" #include "task_controller.hpp" +#include "tractor_facilities.hpp" #include "udp_connections.hpp" class Application @@ -86,6 +87,7 @@ class Application std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; + std::unique_ptr tractorFacilities; std::unique_ptr tecuFunctionalities; std::unique_ptr tcFunctionalities; std::shared_ptr vtClient; @@ -103,6 +105,8 @@ class Application std::int32_t lastXteValue = 0; std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; + + bool tractorFacilitiesSentOnPowerUp = false; std::uint32_t vtDisconnectedSinceMs = 0; std::uint32_t lastVtStatusUpdateMs = 0; std::uint32_t lastVtSectionUpdateMs = 0; diff --git a/include/tractor_facilities.hpp b/include/tractor_facilities.hpp new file mode 100644 index 0000000..6173cc6 --- /dev/null +++ b/include/tractor_facilities.hpp @@ -0,0 +1,158 @@ +/** + * @brief ISO 11783-7 Tractor Facilities (PGN 65033) and Required Tractor + * Facilities (PGN 65032) support. + * + * Builds the 8-byte facility payload from the TECU's live broadcast state + * and registers a PGN-request callback so that implements can query which + * tractor facilities are actually backed by a periodic CAN broadcast. + * + * @see ISO 11783-7:2009, B.24.3 (PGN 65033) and B.24.2 (PGN 65032). + */ + +#pragma once + +#include "isobus/isobus/can_callbacks.hpp" +#include "isobus/isobus/can_control_function.hpp" +#include "isobus/isobus/can_internal_control_function.hpp" +#include "isobus/isobus/isobus_speed_distance_messages.hpp" +#include "isobus/isobus/nmea2000_message_interface.hpp" + +#include +#include +#include +#include +#include + +class Settings; + +/// @brief Describes which tractor facilities are available. +/// Each boolean maps to a single bit in the 8-byte PGN 65033 payload. +/// Field names follow ISO 11783-7:2009 Table B.24. +struct Facilities +{ + // -- Byte 1 ---------------------------------------------------------- + std::uint8_t tecuClass = 1; ///< TECU class (0-3). 00=Class 1, 01=Class 2, 10=Class 3, 11=N/A. + bool engineSpeed = false; ///< Speed information – engine speed + bool groundBasedSpeed = false; ///< Speed information – ground-based speed + bool wheelBasedSpeed = false; ///< Speed information – wheel-based speed + bool powerMaintain = false; ///< Power management – maintain power + bool powerMaxTime = false; ///< Power management – maximum time of tractor power + bool powerKeySwitch = false; ///< Power management – key switch + + // -- Byte 2 ---------------------------------------------------------- + bool languageCommandStorage = false; ///< Language command storage in Tractor ECU + bool minimalLighting = false; ///< Lighting – minimal set as existing trailer connector + bool rearPtoShaftEngagement = false; ///< PTO information – rear shaft engagement + bool rearPtoShaftSpeed = false; ///< PTO information – rear shaft speed + bool rearHitchInWork = false; ///< Hitch information – rear in work + bool rearHitchPosition = false; ///< Hitch information – rear position + + // -- Byte 3 ---------------------------------------------------------- + bool estimatedValveStatus = false; ///< Estimated or measured auxiliary valve status + bool fullImplementLighting = false; ///< Lighting – full implement lighting message set + bool rearDraft = false; ///< Additional hitch parameters – rear draft + bool wheelBasedDirection = false; ///< Speed and distance – wheel-based direction + bool wheelBasedDistance = false; ///< Speed and distance – wheel-based distance + bool groundBasedDirection = false; ///< Speed and distance – ground-based direction + bool groundBasedDistance = false; ///< Speed and distance – ground-based distance + bool timeDate = false; ///< Time/date + + // -- Byte 4 ---------------------------------------------------------- + bool limitRequestStatusReporting = false; ///< Limit/request status reporting + bool auxiliaryValveCommands = false; ///< Auxiliary valve commands + bool rearPtoEngagementCommand = false; ///< PTO commands – rear PTO engagement command + bool rearPtoSpeedCommand = false; ///< PTO commands – rear PTO speed command + bool rearHitchPositionCommand = false; ///< Hitch commands – rear hitch position + + // -- Byte 5 ---------------------------------------------------------- + bool directionControl = false; ///< Direction control + bool selectedSpeedControl = false; ///< Selected speed control + bool selectedSpeed = false; ///< Selected speed + bool operatorExternalLightControls = false; ///< Operator external light controls + // (byte 5 bit 5 is reserved) + bool navigationalPseudoRangeNoise = false; ///< Navigational pseudo-range noise statistics + bool navigationalPositionData = false; ///< Navigational system position data + bool navigationalHighOutputPosition = false; ///< Navigational system high-output position + + // -- Byte 6 ---------------------------------------------------------- + bool frontPtoEngagementCommand = false; ///< PTO commands – front PTO engagement command + bool frontPtoSpeedCommand = false; ///< PTO commands – front PTO speed command + bool frontHitchPositionCommand = false; ///< Hitch commands – front hitch position + bool frontDraft = false; ///< Additional hitch parameters – front draft + bool frontPtoShaftEngagement = false; ///< PTO information – front shaft engagement + bool frontPtoShaftSpeed = false; ///< PTO information – front shaft speed + bool frontHitchInWork = false; ///< Hitch information – front in work + bool frontHitchPosition = false; ///< Hitch information – front position +}; + +/// @brief Encode a Facilities struct into the 8-byte PGN 65033 payload. +/// Reserved bits and byte 7 / byte 8 (including the reserved-bit indicator +/// at byte 8 bit 1) are always 0. +std::array encode_facilities(const Facilities &f); + +/// @brief Decode an 8-byte PGN 65033 payload into a Facilities struct. +Facilities decode_facilities(const std::array &payload); + +/// @brief Implements ISO 11783-7 Tractor Facilities response (PGN 65033) +/// and Required Tractor Facilities diagnostic logging (PGN 65032). +class TractorFacilities +{ +public: + static constexpr std::uint32_t PGN_TRACTOR_FACILITIES = 65033; ///< 0xFE09 + static constexpr std::uint32_t PGN_REQUIRED_TRACTOR_FACILITIES = 65032; ///< 0xFE08 + + TractorFacilities(std::shared_ptr tecuCF, + std::shared_ptr settings); + + /// @brief Register the PGN 65033 request callback and the PGN 65032 + /// global receive listener. Must be called after the TECU + /// address claim has completed. + bool initialize(); + + /// @brief Broadcast PGN 65033 once, either as the unsolicited power-up + /// transmission or in response to a PGN 65033 request. + /// @param isPowerUp Only affects the wording of the confirmation log line. + bool send_facilities_response(bool isPowerUp = true); + + /// @brief Provide a pointer to the speed messages interface so the + /// payload builder can check which speed PGNs are actively + /// being broadcast. + void set_speed_messages_interface(isobus::SpeedMessagesInterface *iface); + + /// @brief Provide a pointer to the NMEA 2000 message interface. + void set_nmea2000_message_interface(isobus::NMEA2000MessageInterface *iface); + + /// @brief Set whether time/date (PGN 65254 / FEE6) is being broadcast. + /// When true, the time/date facility bit is advertised in PGN 65033. + void set_time_date_active(bool active); + + /// @brief Build the 8-byte payload from the current runtime state. + std::array build_payload() const; + +private: + /// @brief Static PGN-request callback registered for PGN 65033. + static bool on_pgn_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + isobus::AcknowledgementType &acknowledgeType, + void *parentPointer); + + /// @brief Static global-PGN callback for PGN 65032 (Required Tractor + /// Facilities). Diagnostic only — logs the request unconditionally + /// (not gated by log level) and never changes our response. + static void on_required_facilities(const isobus::CANMessage &message, void *parentPointer); + + std::shared_ptr tecuCF; + std::shared_ptr settings; + isobus::SpeedMessagesInterface *speedMessagesInterface = nullptr; + isobus::NMEA2000MessageInterface *nmea2000MessageInterface = nullptr; + /// Whether we are broadcasting PGN 65254 (FEE6). Atomic because + /// set_time_date_active() and build_payload() are called from different + /// isobus callback contexts (see docs/CONCURRENCY.md) that can run + /// concurrently on different threads. + std::atomic timeDateActive{ false }; + + /// Source addresses for which we have already logged an info-level + /// "request received" line, to avoid per-message spam. + std::set loggedRequesters; +}; diff --git a/src/app.cpp b/src/app.cpp index be27cb6..0873488 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -13,6 +13,7 @@ #include "isobus/hardware_integration/can_hardware_interface.hpp" #include "isobus/isobus/can_internal_control_function.hpp" #include "isobus/isobus/can_network_manager.hpp" +#include "isobus/isobus/can_parameter_group_number_request_protocol.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_preferred_addresses.hpp" #include "isobus/isobus/isobus_standard_data_description_indices.hpp" @@ -21,6 +22,7 @@ #include "isobus/utility/system_timing.hpp" #include "task_controller.hpp" +#include "tractor_facilities.hpp" #include "AOG_TC.iop.h" @@ -44,6 +46,59 @@ static std::string format_hex_address(std::uint8_t address) return value.str(); } +// Diagnostic callback: log any Request for Repetition Rate (PGN 0xCC00) +// that is properly addressed to one of our control functions. We never +// comply (return false) – this is purely for bus analysis. +static bool log_repetition_rate_request( + std::uint32_t requestedPGN, + std::shared_ptr requestingCF, + std::shared_ptr /*targetCF*/, + std::uint32_t repetitionRate, + void * /*parentPointer*/) +{ + std::uint8_t srcAddr = requestingCF ? requestingCF->get_address() : 0xFF; + std::cout << "[" << get_timestamp() << "] [Diag] Repetition-rate request from SA " + << static_cast(srcAddr) + << ": PGN " << requestedPGN + << " (0x" << std::hex << requestedPGN << std::dec + << "), rate=" << repetitionRate << " ms" << std::endl; + return false; // We don't comply; just logging. +} + +// Diagnostic callback: log broadcast PGN 0xCC00 messages that AgIsoStack +// warns about ("malformed or broadcast request for repetition rate"). +// These bypass the per-ICF callback, so we catch them with a global +// PGN listener. +static void log_broadcast_repetition_rate( + const isobus::CANMessage &message, + void * /*parentPointer*/) +{ + const auto &data = message.get_data(); + if (data.size() < 8) + { + return; + } + + auto sourceCF = message.get_source_control_function(); + std::uint8_t srcAddr = sourceCF ? sourceCF->get_address() : 0xFF; + + // PGN is a 3-byte little-endian value at bytes 0-2. + std::uint32_t requestedPGN = + static_cast(data[0]) | + (static_cast(data[1]) << 8) | + (static_cast(data[2]) << 16); + std::uint16_t requestedRate = + static_cast(data[3]) | + (static_cast(data[4]) << 8); + + std::cout << "[" << get_timestamp() << "] [Diag] Broadcast repetition-rate request from SA " + << static_cast(srcAddr) + << ": PGN " << requestedPGN + << " (0x" << std::hex << requestedPGN << std::dec + << "), rate=" << requestedRate << " ms" + << " (broadcast – AgIsoStack will warn and ignore)" << std::endl; +} + // Enumerate and log all Control Functions on the bus static void enumerate_bus_control_functions(const std::string &context) { @@ -431,6 +486,24 @@ void Application::setup_tecu_interfaces() nmea2000MessageInterface = std::make_unique(tecuCF, settings->is_nmea_send_enabled(), false, false, false, false, false, false); nmea2000MessageInterface->initialize(); log("Init") << "NMEA2000 Message Interface created and initialized." << std::endl; + + // Initialize Tractor Facilities (PGN 65033 / 65032) + tractorFacilities = std::make_unique(tecuCF, settings); + tractorFacilities->set_speed_messages_interface(speedMessagesInterface.get()); + tractorFacilities->set_nmea2000_message_interface(nmea2000MessageInterface.get()); + tractorFacilities->initialize(); + + // Register repetition-rate diagnostic on the TECU's PGN request protocol + auto tecuPgnReq = tecuCF->get_pgn_request_protocol().lock(); + if (tecuPgnReq) + { + tecuPgnReq->register_request_for_repetition_rate_callback( + static_cast(isobus::CANLibParameterGroupNumber::Any), &log_repetition_rate_request, nullptr); + } + + // Also catch broadcast PGN 0xCC00 messages that AgIsoStack warns about + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + 0xCC00 /* RequestForRepetitionRate */, &log_broadcast_repetition_rate, nullptr); } else { @@ -573,6 +646,19 @@ bool Application::update() speedMessagesInterface->update(); if (nmea2000MessageInterface) nmea2000MessageInterface->update(); + + // Transmit PGN 65033 once on power-up (ISO 11783-7 B.24.3 repetition + // rate: "on power-up, and then on request"). + // The send fails until the TECU has claimed an address (or if the transmit queue rejects it), + // so only stop once it went out and retry once a second until then. + static std::uint32_t lastFacilitiesAttemptMs = 0; + if (!tractorFacilitiesSentOnPowerUp && tractorFacilities && + isobus::SystemTiming::time_expired_ms(lastFacilitiesAttemptMs, 1000)) + { + lastFacilitiesAttemptMs = isobus::SystemTiming::get_timestamp_ms(); + tractorFacilitiesSentOnPowerUp = tractorFacilities->send_facilities_response(); + } + if (vtClient) update_vt_client(); diff --git a/src/tractor_facilities.cpp b/src/tractor_facilities.cpp new file mode 100644 index 0000000..f071620 --- /dev/null +++ b/src/tractor_facilities.cpp @@ -0,0 +1,452 @@ +/** + * @brief ISO 11783-7 Tractor Facilities (PGN 65033) implementation. + * + * @see ISO 11783-7:2009, B.24.3 (PGN 65033) and B.24.2 (PGN 65032). + */ + +#include "tractor_facilities.hpp" + +#include "isobus/isobus/can_network_manager.hpp" +#include "isobus/isobus/can_parameter_group_number_request_protocol.hpp" +#include "isobus/isobus/can_stack_logger.hpp" +#include "isobus/utility/system_timing.hpp" + +#include "logging_utils.hpp" +#include "settings.hpp" + +#include +#include +#include + +// --------------------------------------------------------------------------- +// Bit helpers +// --------------------------------------------------------------------------- + +/// Set bit @p n (1-based, per ISO convention: bit 1 = LSB) in @p byte. +static inline void set_bit(std::uint8_t &byte, int n) +{ + byte |= static_cast(1u << (n - 1)); +} + +/// Read bit @p n (1-based) from @p byte. +static inline bool get_bit(std::uint8_t byte, int n) +{ + return (byte >> (n - 1)) & 1u; +} + +// --------------------------------------------------------------------------- +// Encode / Decode +// --------------------------------------------------------------------------- + +std::array encode_facilities(const Facilities &f) +{ + std::array p{}; // Zero-initialised: all reserved bits = 0. + + // -- Byte 1 ---------------------------------------------------------- + // Bits 8,7 = TECU class. ISO mapping: 00=Class 1, 01=Class 2, + // 10=Class 3, 11=Not available. The Facilities struct stores the + // class number (1-3) so we subtract 1; anything else becomes 3 (N/A). + std::uint8_t classBits; + switch (f.tecuClass) + { + case 1: + classBits = 0; + break; // 00 = Class 1 + case 2: + classBits = 1; + break; // 01 = Class 2 + case 3: + classBits = 2; + break; // 10 = Class 3 + default: + classBits = 3; + break; // 11 = Not available + } + p[0] = static_cast(classBits << 6); + if (f.powerKeySwitch) + set_bit(p[0], 6); + if (f.powerMaxTime) + set_bit(p[0], 5); + if (f.powerMaintain) + set_bit(p[0], 4); + if (f.wheelBasedSpeed) + set_bit(p[0], 3); + if (f.groundBasedSpeed) + set_bit(p[0], 2); + if (f.engineSpeed) + set_bit(p[0], 1); + + // -- Byte 2 ---------------------------------------------------------- + if (f.rearHitchPosition) + set_bit(p[1], 8); + if (f.rearHitchInWork) + set_bit(p[1], 7); + if (f.rearPtoShaftSpeed) + set_bit(p[1], 6); + if (f.rearPtoShaftEngagement) + set_bit(p[1], 5); + if (f.minimalLighting) + set_bit(p[1], 4); + if (f.languageCommandStorage) + set_bit(p[1], 3); + // bits 2,1 reserved → 0 + + // -- Byte 3 ---------------------------------------------------------- + if (f.timeDate) + set_bit(p[2], 8); + if (f.groundBasedDistance) + set_bit(p[2], 7); + if (f.groundBasedDirection) + set_bit(p[2], 6); + if (f.wheelBasedDistance) + set_bit(p[2], 5); + if (f.wheelBasedDirection) + set_bit(p[2], 4); + if (f.rearDraft) + set_bit(p[2], 3); + if (f.fullImplementLighting) + set_bit(p[2], 2); + if (f.estimatedValveStatus) + set_bit(p[2], 1); + + // -- Byte 4 ---------------------------------------------------------- + if (f.rearHitchPositionCommand) + set_bit(p[3], 8); + if (f.rearPtoSpeedCommand) + set_bit(p[3], 7); + if (f.rearPtoEngagementCommand) + set_bit(p[3], 6); + if (f.auxiliaryValveCommands) + set_bit(p[3], 5); + if (f.limitRequestStatusReporting) + set_bit(p[3], 4); + // bits 3-1 reserved → 0 + + // -- Byte 5 ---------------------------------------------------------- + if (f.navigationalHighOutputPosition) + set_bit(p[4], 8); + if (f.navigationalPositionData) + set_bit(p[4], 7); + if (f.navigationalPseudoRangeNoise) + set_bit(p[4], 6); + // bit 5 reserved → 0 + if (f.operatorExternalLightControls) + set_bit(p[4], 4); + if (f.selectedSpeed) + set_bit(p[4], 3); + if (f.selectedSpeedControl) + set_bit(p[4], 2); + if (f.directionControl) + set_bit(p[4], 1); + + // -- Byte 6 ---------------------------------------------------------- + if (f.frontHitchPosition) + set_bit(p[5], 8); + if (f.frontHitchInWork) + set_bit(p[5], 7); + if (f.frontPtoShaftSpeed) + set_bit(p[5], 6); + if (f.frontPtoShaftEngagement) + set_bit(p[5], 5); + if (f.frontDraft) + set_bit(p[5], 4); + if (f.frontHitchPositionCommand) + set_bit(p[5], 3); + if (f.frontPtoSpeedCommand) + set_bit(p[5], 2); + if (f.frontPtoEngagementCommand) + set_bit(p[5], 1); + + // -- Byte 7 ---------------------------------------------------------- + // Entirely reserved → 0 + + // -- Byte 8 ---------------------------------------------------------- + // Bits 8-2 reserved → 0. + // Bit 1 (reserved-bit indicator) → 0, signalling modern 0-fill convention. + + return p; +} + +Facilities decode_facilities(const std::array &p) +{ + Facilities f; + + // -- Byte 1 ---------------------------------------------------------- + std::uint8_t classBits = static_cast((p[0] >> 6) & 0x03); + switch (classBits) + { + case 0: + f.tecuClass = 1; + break; // 00 = Class 1 + case 1: + f.tecuClass = 2; + break; // 01 = Class 2 + case 2: + f.tecuClass = 3; + break; // 10 = Class 3 + default: + f.tecuClass = 0; + break; // 11 = Not available + } + f.powerKeySwitch = get_bit(p[0], 6); + f.powerMaxTime = get_bit(p[0], 5); + f.powerMaintain = get_bit(p[0], 4); + f.wheelBasedSpeed = get_bit(p[0], 3); + f.groundBasedSpeed = get_bit(p[0], 2); + f.engineSpeed = get_bit(p[0], 1); + + // -- Byte 2 ---------------------------------------------------------- + f.rearHitchPosition = get_bit(p[1], 8); + f.rearHitchInWork = get_bit(p[1], 7); + f.rearPtoShaftSpeed = get_bit(p[1], 6); + f.rearPtoShaftEngagement = get_bit(p[1], 5); + f.minimalLighting = get_bit(p[1], 4); + f.languageCommandStorage = get_bit(p[1], 3); + + // -- Byte 3 ---------------------------------------------------------- + f.timeDate = get_bit(p[2], 8); + f.groundBasedDistance = get_bit(p[2], 7); + f.groundBasedDirection = get_bit(p[2], 6); + f.wheelBasedDistance = get_bit(p[2], 5); + f.wheelBasedDirection = get_bit(p[2], 4); + f.rearDraft = get_bit(p[2], 3); + f.fullImplementLighting = get_bit(p[2], 2); + f.estimatedValveStatus = get_bit(p[2], 1); + + // -- Byte 4 ---------------------------------------------------------- + f.rearHitchPositionCommand = get_bit(p[3], 8); + f.rearPtoSpeedCommand = get_bit(p[3], 7); + f.rearPtoEngagementCommand = get_bit(p[3], 6); + f.auxiliaryValveCommands = get_bit(p[3], 5); + f.limitRequestStatusReporting = get_bit(p[3], 4); + + // -- Byte 5 ---------------------------------------------------------- + f.navigationalHighOutputPosition = get_bit(p[4], 8); + f.navigationalPositionData = get_bit(p[4], 7); + f.navigationalPseudoRangeNoise = get_bit(p[4], 6); + f.operatorExternalLightControls = get_bit(p[4], 4); + f.selectedSpeed = get_bit(p[4], 3); + f.selectedSpeedControl = get_bit(p[4], 2); + f.directionControl = get_bit(p[4], 1); + + // -- Byte 6 ---------------------------------------------------------- + f.frontHitchPosition = get_bit(p[5], 8); + f.frontHitchInWork = get_bit(p[5], 7); + f.frontPtoShaftSpeed = get_bit(p[5], 6); + f.frontPtoShaftEngagement = get_bit(p[5], 5); + f.frontDraft = get_bit(p[5], 4); + f.frontHitchPositionCommand = get_bit(p[5], 3); + f.frontPtoSpeedCommand = get_bit(p[5], 2); + f.frontPtoEngagementCommand = get_bit(p[5], 1); + + return f; +} + +// --------------------------------------------------------------------------- +// TractorFacilities +// --------------------------------------------------------------------------- + +TractorFacilities::TractorFacilities( + std::shared_ptr tecuCF, + std::shared_ptr settings) : + tecuCF(std::move(tecuCF)), + settings(std::move(settings)) +{ +} + +void TractorFacilities::set_speed_messages_interface(isobus::SpeedMessagesInterface *iface) +{ + speedMessagesInterface = iface; +} + +void TractorFacilities::set_nmea2000_message_interface(isobus::NMEA2000MessageInterface *iface) +{ + nmea2000MessageInterface = iface; +} + +void TractorFacilities::set_time_date_active(bool active) +{ + timeDateActive = active; +} + +std::array TractorFacilities::build_payload() const +{ + Facilities f; + f.tecuClass = 1; // Class 1 – the only class we claim. + + // Ground-based speed (PGN 65097 / 0xFE49) is always broadcast when + // the SpeedMessagesInterface exists. + const bool groundSpeedActive = (speedMessagesInterface != nullptr); + f.groundBasedSpeed = groundSpeedActive; + f.groundBasedDistance = groundSpeedActive; + f.groundBasedDirection = groundSpeedActive; + + // Wheel-based speed (PGN 65096 / 0xFE48) is also always broadcast + // when the interface exists (constructor parameter is `true`). + const bool wheelSpeedActive = (speedMessagesInterface != nullptr); + f.wheelBasedSpeed = wheelSpeedActive; + f.wheelBasedDistance = wheelSpeedActive; + f.wheelBasedDirection = wheelSpeedActive; + + // Time/date (PGN 65254 / FEE6) – set by the application layer when + // the TECU is actively broadcasting FEE6 and no duplicate provider + // exists on the bus. + f.timeDate = timeDateActive; + + // Everything else stays 0: we have no engine data, hitch feedback, + // PTO, auxiliary valves, lighting, language storage (PGN 65039), + // selected speed (PGN 65265), or NMEA 2000 + // position forwarding over Fast Packet. + + return encode_facilities(f); +} + +bool TractorFacilities::initialize() +{ + if (!tecuCF || !tecuCF->get_address_valid()) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] TECU not available, skipping initialization." << std::endl; + return false; + } + + // Register PGN 65033 request callback on the TECU's ICF. + auto pgnRequestProtocol = tecuCF->get_pgn_request_protocol().lock(); + if (!pgnRequestProtocol) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] PGN request protocol not available for TECU." << std::endl; + return false; + } + + bool registered = pgnRequestProtocol->register_pgn_request_callback( + PGN_TRACTOR_FACILITIES, &TractorFacilities::on_pgn_request, this); + if (!registered) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Failed to register PGN 65033 request callback." << std::endl; + return false; + } + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Registered PGN 65033 request callback on TECU (SA " + << static_cast(tecuCF->get_address()) << ")." << std::endl; + + // Register a global receive handler for PGN 65032 (Required Tractor + // Facilities) – diagnostic logging only. + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Registered PGN 65032 diagnostic listener." << std::endl; + + return true; +} + +bool TractorFacilities::send_facilities_response(bool isPowerUp) +{ + if (!tecuCF || !tecuCF->get_address_valid()) + { + return false; + } + + auto payload = build_payload(); + bool sent = isobus::CANNetworkManager::CANNetwork.send_can_message( + PGN_TRACTOR_FACILITIES, payload.data(), payload.size(), tecuCF); + + if (sent) + { + std::ostringstream hex; + hex << std::hex; + for (std::size_t i = 0; i < payload.size(); ++i) + { + if (i != 0) + hex << ' '; + hex << "0x" << static_cast(payload[i]); + } + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Sent PGN 65033 (" + << (isPowerUp ? "power-up" : "requested") << "): [" << hex.str() << "]" << std::endl; + } + else + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Failed to send PGN 65033." << std::endl; + } + return sent; +} + +// --------------------------------------------------------------------------- +// Static callbacks +// --------------------------------------------------------------------------- + +bool TractorFacilities::on_pgn_request( + std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + isobus::AcknowledgementType & /*acknowledgeType*/, + void *parentPointer) +{ + if (parameterGroupNumber != PGN_TRACTOR_FACILITIES || !parentPointer || !requestingControlFunction) + { + return false; + } + + auto *self = static_cast(parentPointer); + + // Sending the requested PGN *is* the response; an ACK on top would be + // redundant and confusing to some implement stacks. + acknowledge = false; + + // Log once per requester source address at info level. + std::uint8_t sa = requestingControlFunction->get_address(); + if (self->loggedRequesters.find(sa) == self->loggedRequesters.end()) + { + self->loggedRequesters.insert(sa); + + auto payload = self->build_payload(); + std::ostringstream hex; + hex << std::hex; + for (std::size_t i = 0; i < payload.size(); ++i) + { + if (i != 0) + hex << ' '; + hex << "0x" << static_cast(payload[i]); + } + std::cout << "[" << get_timestamp() << "] [TractorFacilities] PGN 65033 requested by SA " + << static_cast(sa) << ", advertising facilities: [" << hex.str() << "]" << std::endl; + } + + return self->send_facilities_response(false); +} + +void TractorFacilities::on_required_facilities( + const isobus::CANMessage &message, + void *parentPointer) +{ + if (!parentPointer) + { + return; + } + + const auto &data = message.get_data(); + if (data.size() < 8) + { + return; + } + + auto sourceCF = message.get_source_control_function(); + std::uint8_t sa = sourceCF ? sourceCF->get_address() : 0xFF; + + // Decode and log — diagnostic only, do not change our + // response based on what the implement asks for. + std::array raw{}; + for (std::size_t i = 0; i < 8 && i < data.size(); ++i) + { + raw[i] = data[i]; + } + + // A simple hex dump avoids pulling in the full Facilities decode for a + // diagnostic message. + std::ostringstream hex; + hex << std::hex; + for (std::size_t i = 0; i < raw.size(); ++i) + { + if (i != 0) + hex << ' '; + hex << "0x" << static_cast(raw[i]); + } + isobus::CANStackLogger::debug("[TractorFacilities] PGN 65032 from SA " + std::to_string(static_cast(sa)) + + ": required facilities [" + hex.str() + "]"); +} diff --git a/tools/test_tractor_facilities.cpp b/tools/test_tractor_facilities.cpp new file mode 100644 index 0000000..e88644f --- /dev/null +++ b/tools/test_tractor_facilities.cpp @@ -0,0 +1,345 @@ +/** + * @brief Unit tests for the ISO 11783-7 Tractor Facilities (PGN 65033) + * encode / decode logic. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "tractor_facilities.hpp" + +#include +#include +#include +#include +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +// --------------------------------------------------------------------------- +// Test 1: Default configuration – ground-based speed only +// --------------------------------------------------------------------------- +static void test_default_ground_speed_only() +{ + std::printf("test_default_ground_speed_only\n"); + + Facilities f; + f.tecuClass = 1; + f.groundBasedSpeed = true; + f.groundBasedDistance = true; + f.groundBasedDirection = true; + + auto payload = encode_facilities(f); + + // Byte 1: TECU class 1 → bits 8,7 = 00; ground-based speed (ISO bit 2) → C++ bit 1 + // → 0b00000010 = 0x02 + check(payload[0] == 0x02, "byte 1 = 0x02 (class 1 + ground-based speed)"); + + // Byte 3: ground-based distance (ISO bit 7 = C++ bit 6 = 0x40) + // + ground-based direction (ISO bit 6 = C++ bit 5 = 0x20) + // → 0b01100000 = 0x60 + check(payload[2] == 0x60, "byte 3 = 0x60 (ground-based distance + direction)"); + + // All other bytes must be 0 + check(payload[1] == 0x00, "byte 2 = 0x00"); + check(payload[3] == 0x00, "byte 4 = 0x00"); + check(payload[4] == 0x00, "byte 5 = 0x00"); + check(payload[5] == 0x00, "byte 6 = 0x00"); + check(payload[6] == 0x00, "byte 7 = 0x00 (reserved)"); + check(payload[7] == 0x00, "byte 8 = 0x00 (reserved + reserved-bit indicator)"); +} + +// --------------------------------------------------------------------------- +// Test 2: Full default – ground + wheel speed (our TECU's actual default) +// --------------------------------------------------------------------------- +static void test_full_default() +{ + std::printf("test_full_default\n"); + + Facilities f; + f.tecuClass = 1; + f.groundBasedSpeed = true; + f.groundBasedDistance = true; + f.groundBasedDirection = true; + f.wheelBasedSpeed = true; + f.wheelBasedDistance = true; + f.wheelBasedDirection = true; + + auto payload = encode_facilities(f); + + // Byte 1: class 1 (00) + wheel-based (ISO bit 3 = C++ bit 2 = 0x04) + // + ground-based (ISO bit 2 = C++ bit 1 = 0x02) + // → 0b00000110 = 0x06 + check(payload[0] == 0x06, "byte 1 = 0x06 (class 1 + wheel + ground speed)"); + + // Byte 3: ground dist (ISO bit 7 = 0x40) + ground dir (ISO bit 6 = 0x20) + // + wheel dist (ISO bit 5 = 0x10) + wheel dir (ISO bit 4 = 0x08) + // → 0b01111000 = 0x78 + check(payload[2] == 0x78, "byte 3 = 0x78 (ground + wheel distance/direction)"); + + check(payload[1] == 0x00, "byte 2 = 0x00"); + check(payload[3] == 0x00, "byte 4 = 0x00"); + check(payload[4] == 0x00, "byte 5 = 0x00"); + check(payload[5] == 0x00, "byte 6 = 0x00"); + check(payload[6] == 0x00, "byte 7 = 0x00"); + check(payload[7] == 0x00, "byte 8 = 0x00"); +} + +// --------------------------------------------------------------------------- +// Test 3: Every reserved bit is 0, including byte 8 bit 1 +// --------------------------------------------------------------------------- +static void test_reserved_bits_zero() +{ + std::printf("test_reserved_bits_zero\n"); + + // Set every non-reserved facility to true. + Facilities f; + f.tecuClass = 1; + f.engineSpeed = true; + f.groundBasedSpeed = true; + f.wheelBasedSpeed = true; + f.powerMaintain = true; + f.powerMaxTime = true; + f.powerKeySwitch = true; + f.rearHitchPosition = true; + f.rearPtoShaftSpeed = true; + f.rearPtoShaftEngagement = true; + f.minimalLighting = true; + f.languageCommandStorage = true; + f.timeDate = true; + f.groundBasedDistance = true; + f.groundBasedDirection = true; + f.wheelBasedDistance = true; + f.wheelBasedDirection = true; + f.rearDraft = true; + f.fullImplementLighting = true; + f.estimatedValveStatus = true; + f.rearHitchPositionCommand = true; + f.rearPtoSpeedCommand = true; + f.rearPtoEngagementCommand = true; + f.auxiliaryValveCommands = true; + f.limitRequestStatusReporting = true; + f.navigationalHighOutputPosition = true; + f.navigationalPositionData = true; + f.navigationalPseudoRangeNoise = true; + f.operatorExternalLightControls = true; + f.selectedSpeed = true; + f.selectedSpeedControl = true; + f.directionControl = true; + f.frontHitchPosition = true; + f.frontPtoShaftSpeed = true; + f.frontPtoShaftEngagement = true; + f.frontDraft = true; + f.frontHitchPositionCommand = true; + f.frontPtoSpeedCommand = true; + f.frontPtoEngagementCommand = true; + + auto payload = encode_facilities(f); + + // Byte 2 bits 2,1 are reserved → must be 0 + check((payload[1] & 0x03) == 0x00, "byte 2 bits 2,1 reserved = 0"); + + // Byte 4 bits 3-1 are reserved → must be 0 + check((payload[3] & 0x07) == 0x00, "byte 4 bits 3-1 reserved = 0"); + + // Byte 5 bit 5 is reserved → must be 0 + check((payload[4] & 0x10) == 0x00, "byte 5 bit 5 reserved = 0"); + + // Byte 7 is entirely reserved → must be 0 + check(payload[6] == 0x00, "byte 7 entirely reserved = 0x00"); + + // Byte 8 is entirely reserved (including bit 1 reserved-bit indicator) → must be 0 + check(payload[7] == 0x00, "byte 8 entirely reserved = 0x00 (including reserved-bit indicator)"); + + // TECU class bits should be 00 (class 1) in bits 8,7 + check((payload[0] & 0xC0) == 0x00, "TECU class = 00 (class 1) in byte 1 bits 8,7"); +} + +// --------------------------------------------------------------------------- +// Test 4: Disabled broadcast clears its facility bit +// --------------------------------------------------------------------------- +static void test_disabled_broadcast_clears_bit() +{ + std::printf("test_disabled_broadcast_clears_bit\n"); + + // Start with ground-based speed enabled. + Facilities f; + f.tecuClass = 1; + f.groundBasedSpeed = true; + f.groundBasedDistance = true; + f.groundBasedDirection = true; + + auto payloadEnabled = encode_facilities(f); + check(payloadEnabled[0] == 0x02, "ground-based speed bit set when enabled"); + check(payloadEnabled[2] == 0x60, "ground-based distance/direction bits set when enabled"); + + // Now disable ground-based speed. + f.groundBasedSpeed = false; + f.groundBasedDistance = false; + f.groundBasedDirection = false; + + auto payloadDisabled = encode_facilities(f); + check(payloadDisabled[0] == 0x00, "ground-based speed bit cleared when disabled"); + check(payloadDisabled[2] == 0x00, "ground-based distance/direction bits cleared when disabled"); +} + +// --------------------------------------------------------------------------- +// Test 5: Round-trip encode → decode +// --------------------------------------------------------------------------- +static void test_round_trip() +{ + std::printf("test_round_trip\n"); + + Facilities original; + original.tecuClass = 2; + original.engineSpeed = true; + original.groundBasedSpeed = true; + original.wheelBasedSpeed = false; + original.powerKeySwitch = true; + original.rearHitchPosition = true; + original.languageCommandStorage = true; + original.timeDate = true; + original.groundBasedDistance = true; + original.wheelBasedDirection = true; + original.rearDraft = true; + original.fullImplementLighting = true; + original.rearHitchPositionCommand = true; + original.auxiliaryValveCommands = true; + original.navigationalPositionData = true; + original.selectedSpeed = true; + original.directionControl = true; + original.frontHitchPosition = true; + original.frontPtoShaftEngagement = true; + original.frontPtoSpeedCommand = true; + + auto payload = encode_facilities(original); + Facilities decoded = decode_facilities(payload); + + check(decoded.tecuClass == original.tecuClass, "tecuClass round-trip"); + check(decoded.engineSpeed == original.engineSpeed, "engineSpeed round-trip"); + check(decoded.groundBasedSpeed == original.groundBasedSpeed, "groundBasedSpeed round-trip"); + check(decoded.wheelBasedSpeed == original.wheelBasedSpeed, "wheelBasedSpeed round-trip"); + check(decoded.powerKeySwitch == original.powerKeySwitch, "powerKeySwitch round-trip"); + check(decoded.powerMaxTime == original.powerMaxTime, "powerMaxTime round-trip"); + check(decoded.powerMaintain == original.powerMaintain, "powerMaintain round-trip"); + check(decoded.rearHitchPosition == original.rearHitchPosition, "rearHitchPosition round-trip"); + check(decoded.rearHitchInWork == original.rearHitchInWork, "rearHitchInWork round-trip"); + check(decoded.rearPtoShaftSpeed == original.rearPtoShaftSpeed, "rearPtoShaftSpeed round-trip"); + check(decoded.rearPtoShaftEngagement == original.rearPtoShaftEngagement, "rearPtoShaftEngagement round-trip"); + check(decoded.minimalLighting == original.minimalLighting, "minimalLighting round-trip"); + check(decoded.languageCommandStorage == original.languageCommandStorage, "languageCommandStorage round-trip"); + check(decoded.timeDate == original.timeDate, "timeDate round-trip"); + check(decoded.groundBasedDistance == original.groundBasedDistance, "groundBasedDistance round-trip"); + check(decoded.groundBasedDirection == original.groundBasedDirection, "groundBasedDirection round-trip"); + check(decoded.wheelBasedDistance == original.wheelBasedDistance, "wheelBasedDistance round-trip"); + check(decoded.wheelBasedDirection == original.wheelBasedDirection, "wheelBasedDirection round-trip"); + check(decoded.rearDraft == original.rearDraft, "rearDraft round-trip"); + check(decoded.fullImplementLighting == original.fullImplementLighting, "fullImplementLighting round-trip"); + check(decoded.estimatedValveStatus == original.estimatedValveStatus, "estimatedValveStatus round-trip"); + check(decoded.rearHitchPositionCommand == original.rearHitchPositionCommand, "rearHitchPositionCommand round-trip"); + check(decoded.rearPtoSpeedCommand == original.rearPtoSpeedCommand, "rearPtoSpeedCommand round-trip"); + check(decoded.rearPtoEngagementCommand == original.rearPtoEngagementCommand, "rearPtoEngagementCommand round-trip"); + check(decoded.auxiliaryValveCommands == original.auxiliaryValveCommands, "auxiliaryValveCommands round-trip"); + check(decoded.limitRequestStatusReporting == original.limitRequestStatusReporting, "limitRequestStatusReporting round-trip"); + check(decoded.navigationalHighOutputPosition == original.navigationalHighOutputPosition, "navigationalHighOutputPosition round-trip"); + check(decoded.navigationalPositionData == original.navigationalPositionData, "navigationalPositionData round-trip"); + check(decoded.navigationalPseudoRangeNoise == original.navigationalPseudoRangeNoise, "navigationalPseudoRangeNoise round-trip"); + check(decoded.operatorExternalLightControls == original.operatorExternalLightControls, "operatorExternalLightControls round-trip"); + check(decoded.selectedSpeed == original.selectedSpeed, "selectedSpeed round-trip"); + check(decoded.selectedSpeedControl == original.selectedSpeedControl, "selectedSpeedControl round-trip"); + check(decoded.directionControl == original.directionControl, "directionControl round-trip"); + check(decoded.frontHitchPosition == original.frontHitchPosition, "frontHitchPosition round-trip"); + check(decoded.frontHitchInWork == original.frontHitchInWork, "frontHitchInWork round-trip"); + check(decoded.frontPtoShaftSpeed == original.frontPtoShaftSpeed, "frontPtoShaftSpeed round-trip"); + check(decoded.frontPtoShaftEngagement == original.frontPtoShaftEngagement, "frontPtoShaftEngagement round-trip"); + check(decoded.frontDraft == original.frontDraft, "frontDraft round-trip"); + check(decoded.frontHitchPositionCommand == original.frontHitchPositionCommand, "frontHitchPositionCommand round-trip"); + check(decoded.frontPtoSpeedCommand == original.frontPtoSpeedCommand, "frontPtoSpeedCommand round-trip"); + check(decoded.frontPtoEngagementCommand == original.frontPtoEngagementCommand, "frontPtoEngagementCommand round-trip"); +} + +// --------------------------------------------------------------------------- +// Test 6: TECU class not available encoding +// --------------------------------------------------------------------------- +static void test_tecu_class_not_available() +{ + std::printf("test_tecu_class_not_available\n"); + + Facilities f; + f.tecuClass = 0; // 0 = not available (bit pattern 11) + + auto payload = encode_facilities(f); + // Byte 1 bits 8,7 = 11 → 0b11000000 = 0xC0 + check(payload[0] == 0xC0, "byte 1 = 0xC0 (TECU class not available)"); +} + +// --------------------------------------------------------------------------- +// Test 7: All-zero payload decodes to default Facilities +// --------------------------------------------------------------------------- +static void test_zero_payload() +{ + std::printf("test_zero_payload\n"); + + std::array zero{}; + Facilities f = decode_facilities(zero); + + check(f.tecuClass == 1, "tecuClass = 1 (class 1) from zero payload"); + check(!f.engineSpeed, "engineSpeed = false from zero payload"); + check(!f.groundBasedSpeed, "groundBasedSpeed = false from zero payload"); + check(!f.wheelBasedSpeed, "wheelBasedSpeed = false from zero payload"); + check(!f.rearHitchInWork, "rearHitchInWork = false from zero payload"); + check(!f.languageCommandStorage, "languageCommandStorage = false from zero payload"); + check(!f.timeDate, "timeDate = false from zero payload"); + check(!f.selectedSpeed, "selectedSpeed = false from zero payload"); + check(!f.navigationalPositionData, "navigationalPositionData = false from zero payload"); + check(!f.frontHitchPosition, "frontHitchPosition = false from zero payload"); +} + +// --------------------------------------------------------------------------- +// Test 8: Rear hitch in work is NOT set by default (safety check) +// --------------------------------------------------------------------------- +static void test_rear_hitch_in_work_not_set() +{ + std::printf("test_rear_hitch_in_work_not_set\n"); + + // Even with everything "on" that we support, rear hitch in work must + // stay 0 unless explicitly requested. + Facilities f; + f.tecuClass = 1; + f.groundBasedSpeed = true; + f.wheelBasedSpeed = true; + + auto payload = encode_facilities(f); + // Byte 2 bit 7 = rear hitch in work → must be 0 + check((payload[1] & 0x40) == 0x00, "rear hitch in work (byte 2 bit 7) = 0 by default"); +} + +int main() +{ + test_default_ground_speed_only(); + test_full_default(); + test_reserved_bits_zero(); + test_disabled_broadcast_clears_bit(); + test_round_trip(); + test_tecu_class_not_available(); + test_zero_payload(); + test_rear_hitch_in_work_not_set(); + + if (failures > 0) + { + std::fprintf(stderr, "\n%d test(s) FAILED\n", failures); + return 1; + } + + std::printf("\nAll tests passed.\n"); + return 0; +} From bd7ff2a629a3a412d460dd414eb8e46586b01dcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 27 Sep 2026 16:48:31 +0200 Subject: [PATCH 05/22] Link async_log into test_guidance_track guidance_track_context.cpp now logs through log(), which needs async_log.cpp; the standalone test target only compiled the parser, so the IOP validation build failed to link. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0d7e2e..0b5af7a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -165,11 +165,13 @@ if(AOG_TC_VALIDATE_IOP) add_executable( test_guidance_track ${CMAKE_CURRENT_LIST_DIR}/tools/test_guidance_track.cpp - ${CMAKE_CURRENT_LIST_DIR}/src/guidance_track_context.cpp) + ${CMAKE_CURRENT_LIST_DIR}/src/guidance_track_context.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/async_log.cpp) target_compile_features(test_guidance_track PRIVATE cxx_std_20) set_target_properties(test_guidance_track PROPERTIES CXX_EXTENSIONS OFF) target_include_directories(test_guidance_track PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + target_link_libraries(test_guidance_track PRIVATE Threads::Threads) add_dependencies(iop_validator test_guidance_track) add_test(NAME guidance_track_parsing COMMAND test_guidance_track) From f3d099824a06094ce1f812bef970cf0709ecbaf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Mon, 28 Sep 2026 22:34:29 +0200 Subject: [PATCH 06/22] Cut log noise from bad UDP packets and global-destination RTS frames "Unknown start of message" was printed twice per packet: both UDP sockets listen on port 8888 (one on the AOG subnet address, one on all interfaces), so every broadcast arrives on both. Log it through UdpConnections::log_unknown_start(), which skips an identical report (same sender, same leading bytes) within 50 ms and includes the sender IP:port and up to 16 bytes in hex, so the source can be found. "Received a Request to Send (RTS) message with a global destination" is a stack warning without a source address, printed for every frame. The CAN stack log sink now treats it as Debug. The application counts such RTS frames per source address from the CAN frame received event and logs one summary line per minute when there were any. Co-Authored-By: Claude Opus 5.5 --- include/udp_connections.hpp | 11 +++++++++ src/app.cpp | 46 +++++++++++++++++++++++++++++++++++++ src/logging.cpp | 12 ++++++++++ src/udp_connections.cpp | 35 ++++++++++++++++++++++++++-- 4 files changed, 102 insertions(+), 2 deletions(-) diff --git a/include/udp_connections.hpp b/include/udp_connections.hpp index 2c2200a..36d2032 100644 --- a/include/udp_connections.hpp +++ b/include/udp_connections.hpp @@ -10,7 +10,9 @@ #pragma once #include +#include #include +#include #include "settings.hpp" using boost::asio::ip::udp; @@ -92,7 +94,16 @@ class UdpConnections */ std::uint8_t calculate_crc(std::span data); + /** + * @brief Log a packet that does not start with PACKET_START, once even if both sockets receive it + * @param sender The endpoint the packet came from + * @param data The bytes from the unexpected start onwards + */ + void log_unknown_start(const udp::endpoint &sender, std::span data); + PacketCallback packetCallback = nullptr; + std::string lastUnknownStartSignature; ///< Sender and leading bytes of the last logged unknown start + std::chrono::steady_clock::time_point lastUnknownStartTime; std::shared_ptr settings; udp::socket udpConnection; udp::socket udpConnectionAddressDetection; diff --git a/src/app.cpp b/src/app.cpp index 9ac8802..d413490 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -27,6 +27,8 @@ #include "logging_utils.hpp" #include +#include +#include #include #include #include @@ -44,6 +46,42 @@ static std::string format_hex_address(std::uint8_t address) return value.str(); } +// Requests to Send with a global destination are invalid and ignored by the stack. Its per-message warning +// has no source address, so count them per source here (CAN thread) and report once per minute from update(). +static std::array, 256> globalRtsCountBySource{}; + +static void count_global_rts(const isobus::CANMessageFrame &frame) +{ + constexpr std::uint8_t TP_CONNECTION_MANAGEMENT_PF = 0xEC; + constexpr std::uint8_t TP_REQUEST_TO_SEND = 16; + if (frame.isExtendedFrame && (frame.dataLength > 0) && + (((frame.identifier >> 16) & 0xFF) == TP_CONNECTION_MANAGEMENT_PF) && + (((frame.identifier >> 8) & 0xFF) == 0xFF) && + (frame.data[0] == TP_REQUEST_TO_SEND)) + { + globalRtsCountBySource[frame.identifier & 0xFF]++; + } +} + +static void report_global_rts_counts() +{ + std::ostringstream sources; + std::uint32_t total = 0; + for (std::size_t source = 0; source < globalRtsCountBySource.size(); source++) + { + const std::uint32_t count = globalRtsCountBySource[source].exchange(0); + if (count > 0) + { + sources << (total > 0 ? ", " : "") << format_hex_address(static_cast(source)) << " (" << count << ")"; + total += count; + } + } + if (total > 0) + { + log("TP") << "Ignored " << total << " Request to Send message(s) with a global destination in the last minute, from source address(es) " << sources.str() << std::endl; + } +} + // Enumerate and log all Control Functions on the bus static void enumerate_bus_control_functions(const std::string &context) { @@ -194,6 +232,7 @@ bool Application::setup_can_hardware() } isobus::CANHardwareInterface::set_number_of_can_channels(1); isobus::CANHardwareInterface::assign_can_channel_frame_handler(0, canDriver); + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener(count_global_rts); canHardwareStarted = isobus::CANHardwareInterface::start(); if ((!canHardwareStarted) || (!canDriver->get_is_valid())) @@ -627,6 +666,13 @@ bool Application::update() lastConflictCheck = isobus::SystemTiming::get_timestamp_ms(); } + static std::uint32_t lastGlobalRtsReportMs = 0; + if (isobus::SystemTiming::time_expired_ms(lastGlobalRtsReportMs, 60000)) + { + report_global_rts_counts(); + lastGlobalRtsReportMs = isobus::SystemTiming::get_timestamp_ms(); + } + // Diff active implement clients once per second so disconnect messages retain prior metadata. static std::uint32_t lastImplementScanMs = 0; if (isobus::SystemTiming::time_expired_ms(lastImplementScanMs, 1000)) diff --git a/src/logging.cpp b/src/logging.cpp index 2534664..37c679f 100644 --- a/src/logging.cpp +++ b/src/logging.cpp @@ -78,6 +78,18 @@ class CustomLogger : public isobus::CANStackLogger void sink_CAN_stack_log(CANStackLogger::LoggingLevel level, const std::string &text) override { + // Some bus devices send these continuously. The stack's text has no source address; the application + // reports them per source once per minute instead (see report_global_rts_counts in app.cpp). + if ((LoggingLevel::Warning == level) && + (text == "[TP]: Received a Request to Send (RTS) message with a global destination, ignoring")) + { + level = LoggingLevel::Debug; + if (get_log_level() > LoggingLevel::Debug) + { + return; + } + } + std::ostream &out = async_log::stream(); out << "[" << get_timestamp() << "] "; switch (level) diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index a4e7540..ad85a22 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -8,8 +8,11 @@ */ #include "udp_connections.hpp" +#include #include +#include #include +#include #include "logging_utils.hpp" #if !defined(_WIN32) @@ -153,6 +156,34 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) return static_cast(result & 0xFF); } +void UdpConnections::log_unknown_start(const udp::endpoint &sender, std::span data) +{ + // Both sockets listen on port 8888 (one on the AOG subnet address, one on all interfaces), so a + // broadcast packet usually arrives on both. Only log it once. + static constexpr std::size_t MAX_LOGGED_BYTES = 16; + static constexpr auto DUPLICATE_WINDOW = std::chrono::milliseconds(50); + + std::ostringstream signature; + signature << sender.address().to_string() << ":" << sender.port() << " " << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < std::min(data.size(), MAX_LOGGED_BYTES); i++) + { + signature << (i ? " " : "") << std::setw(2) << static_cast(data[i]); + } + if (data.size() > MAX_LOGGED_BYTES) + { + signature << " ..."; + } + + const auto now = std::chrono::steady_clock::now(); + if ((signature.str() == lastUnknownStartSignature) && (now - lastUnknownStartTime < DUPLICATE_WINDOW)) + { + return; + } + lastUnknownStartSignature = signature.str(); + lastUnknownStartTime = now; + log() << "Unknown start of message from " << lastUnknownStartSignature << " (" << std::dec << data.size() << " bytes)" << std::endl; +} + void UdpConnections::handle_incoming_packets() { static std::array rxBuffer; @@ -200,7 +231,7 @@ void UdpConnections::handle_incoming_packets() else { // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; + log_unknown_start(sender_endpoint, { rxBuffer.data() + index - 2, rxIndex - (index - 2) }); rxIndex = 0; } @@ -288,7 +319,7 @@ void UdpConnections::handle_address_detection() else { // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; + log_unknown_start(sender_endpoint, { rxBuffer.data() + index - 2, rxIndex - (index - 2) }); rxIndex = 0; } From a11fdade70fe94b87d4578eac7cdf9faf815a484 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Mon, 28 Sep 2026 22:35:17 +0200 Subject: [PATCH 07/22] Log DDOPs without sections as a non-section client instead of a warning A client whose DDOP has no section elements, such as a tractor ECU that only describes hitch or GNSS geometry, has nothing to section-control. It was reported with "WARNING: No supported section control method detected!", which sent people looking for a problem that is not there. Log it as info ("Non-section client") when the DDOP has zero sections; DDOPs that do have sections but no supported method still get the warning. Co-Authored-By: Claude Opus 5.5 --- src/task_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/task_controller.cpp b/src/task_controller.cpp index f60afef..b13a7ef 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -597,6 +597,11 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p log() << " Section " << static_cast(i) << " -> element " << sectionElementNumbers[i] << std::endl; } } + else if (0 == numberOfSections) + { + // e.g. a tractor ECU describing hitch or GNSS geometry: nothing to control, nothing wrong + log("TC Server") << "Non-section client: the DDOP has no section elements, section control not applicable." << std::endl; + } else { log("TC Server") << "WARNING: No supported section control method detected! " From 99abf3d7a25d34af1cd443541f046f795ba541da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Mon, 12 Oct 2026 23:48:22 +0200 Subject: [PATCH 08/22] Switch section control on automatically when an implement connects Until now an implement stayed in manual mode after connecting until the operator pressed AOG's ISOBUS SC button, which sends PGN 241. The TC now remembers the section control state it wants and applies it to each client 2 s after its pool is activated: it sets the local auto flag and writes DDI 160 when the implement declares it. The initial state comes from the new settings.json key "sectionControlOnConnect" (default true). Pressing AOG's button still switches all clients and also becomes the state for implements that connect later. The implement can still decline or go back to manual; its DDI 160 report is followed as before. Co-Authored-By: Claude Opus 5.5 --- include/settings.hpp | 8 +++++ include/task_controller.hpp | 12 ++++++++ src/app.cpp | 1 + src/settings.cpp | 13 ++++++++ src/task_controller.cpp | 61 ++++++++++++++++++++++++++++++------- 5 files changed, 84 insertions(+), 11 deletions(-) diff --git a/include/settings.hpp b/include/settings.hpp index 06f0278..0373b55 100644 --- a/include/settings.hpp +++ b/include/settings.hpp @@ -98,6 +98,12 @@ class Settings */ bool set_vt_enabled(bool enabled, bool save = true); + /// @brief Check whether section control is switched on automatically when an implement connects. + bool is_section_control_on_connect() const; + + /// @brief Set whether section control is switched on automatically when an implement connects. + bool set_section_control_on_connect(bool enabled, bool save = true); + /** * @brief Get the configured TC version * @return The configured version (0-4, default 3) @@ -161,6 +167,7 @@ class Settings constexpr static bool DEFAULT_NMEA_SEND_ENABLED = true; constexpr static bool DEFAULT_AOG_HEARTBEAT_ENABLED = true; constexpr static bool DEFAULT_VT_ENABLED = true; + constexpr static bool DEFAULT_SECTION_CONTROL_ON_CONNECT = true; constexpr static std::uint8_t DEFAULT_TC_VERSION = 3; ///< SecondEditionDraft (V3 default for maximum implement compatibility) static const std::string DEFAULT_LANGUAGE_CODE; static const std::string DEFAULT_COUNTRY_CODE; @@ -169,6 +176,7 @@ class Settings bool nmeaSendEnabled = DEFAULT_NMEA_SEND_ENABLED; bool aogHeartbeatEnabled = DEFAULT_AOG_HEARTBEAT_ENABLED; bool vtEnabled = DEFAULT_VT_ENABLED; + bool sectionControlOnConnect = DEFAULT_SECTION_CONTROL_ON_CONNECT; std::uint8_t tcVersion = DEFAULT_TC_VERSION; std::string languageCode = DEFAULT_LANGUAGE_CODE; std::string countryCode = DEFAULT_COUNTRY_CODE; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 705605e..c590659 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include constexpr std::uint8_t NUMBER_SECTIONS_PER_CONDENSED_MESSAGE = 16; @@ -55,6 +56,10 @@ class ClientState isobus::DeviceDescriptorObjectPool &get_pool(); bool are_measurement_commands_sent() const; void mark_measurement_commands_sent(); + /// @brief Schedules the TC's section control state to be applied to this client at the given time. + void schedule_initial_section_control(std::uint32_t dueMs); + /// @brief Returns true (once) when the scheduled initial section control state is due. + bool take_due_initial_section_control(std::uint32_t nowMs); 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; @@ -66,6 +71,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::optional initialSectionControlDueMs; ///< When to apply the TC's section control state after connect std::map ddiToElementNumber; ///< Mapping of DDI to element number // TODO: better way to do this? std::uint8_t numberOfSections; @@ -116,13 +122,19 @@ class MyTCServer : public isobus::TaskControllerServer void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + /// @brief Sets the section control state applied to implements as they connect, before AOG asks for one. + void set_initial_section_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); + /// @brief Sets the local auto/manual flag and commands DDI 160 if the client supports it. Caller must hold clientsMutex. + void apply_section_control_state(std::shared_ptr client, ClientState &state, bool enabled); bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); std::map, ClientState> clients; std::map, std::queue>> uploadedPools; + bool desiredSectionControlEnabled = false; ///< Applied to implements when they connect; follows AOG's ISOBUS SC button. Guarded by clientsMutex /// @brief Guards clients and uploadedPools. /// diff --git a/src/app.cpp b/src/app.cpp index 9ac8802..fcff617 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -381,6 +381,7 @@ void Application::setup_task_controller_server() languageInterface.set_language_code(settings->get_language_code()); languageInterface.set_country_code(settings->get_country_code()); tcServer->initialize(); + tcServer->set_initial_section_control_enabled(settings->is_section_control_on_connect()); tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG tcFunctionalities = std::make_unique(tcCF); tcFunctionalities->set_functionality_is_supported( diff --git a/src/settings.cpp b/src/settings.cpp index 960927a..8b6c53f 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -70,6 +70,7 @@ bool Settings::load() nmeaSendEnabled = loadBoolean("nmeaSendEnabled", DEFAULT_NMEA_SEND_ENABLED); aogHeartbeatEnabled = loadBoolean("aogHeartbeatEnabled", DEFAULT_AOG_HEARTBEAT_ENABLED); vtEnabled = loadBoolean("vtEnabled", DEFAULT_VT_ENABLED); + sectionControlOnConnect = loadBoolean("sectionControlOnConnect", DEFAULT_SECTION_CONTROL_ON_CONNECT); if (data.contains("tcVersion")) { @@ -143,6 +144,7 @@ bool Settings::save() const data["nmeaSendEnabled"] = nmeaSendEnabled; data["aogHeartbeatEnabled"] = aogHeartbeatEnabled; data["vtEnabled"] = vtEnabled; + data["sectionControlOnConnect"] = sectionControlOnConnect; data["tcVersion"] = tcVersion; data["languageCode"] = languageCode; data["countryCode"] = countryCode; @@ -261,6 +263,17 @@ bool Settings::set_vt_enabled(bool enabled, bool save) return set_boolean(vtEnabled, enabled, save); } +bool Settings::is_section_control_on_connect() const +{ + std::scoped_lock lock(settingsMutex); + return sectionControlOnConnect; +} + +bool Settings::set_section_control_on_connect(bool enabled, bool save) +{ + return set_boolean(sectionControlOnConnect, enabled, save); +} + bool Settings::is_aog_heartbeat_enabled() const { std::scoped_lock lock(settingsMutex); diff --git a/src/task_controller.cpp b/src/task_controller.cpp index f60afef..61f7e6a 100644 --- a/src/task_controller.cpp +++ b/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 "isobus/utility/system_timing.hpp" #include #include @@ -21,6 +22,10 @@ #include #include +/// Delay between pool activation and applying the initial section control state, so the implement +/// has settled and answered the measurement commands before it is switched to automatic. +static constexpr std::uint32_t INITIAL_SECTION_CONTROL_DELAY_MS = 2000; + // Sanitize a string for use as a filename by replacing invalid characters with underscores static std::string sanitize_filename(const std::string &input) { @@ -210,6 +215,22 @@ void ClientState::set_section_control_enabled(bool state) isSectionControlEnabled = state; } +void ClientState::schedule_initial_section_control(std::uint32_t dueMs) +{ + initialSectionControlDueMs = dueMs; +} + +bool ClientState::take_due_initial_section_control(std::uint32_t nowMs) +{ + // Signed difference so the comparison survives timestamp wrap-around + if (!initialSectionControlDueMs || static_cast(nowMs - *initialSectionControlDueMs) < 0) + { + return false; + } + initialSectionControlDueMs.reset(); + return true; +} + bool ClientState::uses_per_element_control() const { return usesPerElementControl; @@ -879,6 +900,15 @@ void MyTCServer::request_measurement_commands() log() << "Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); + + // Switch the implement to automatic shortly after it connects, so AOG's SC button + // does not need pressing on every connect. + client.second.schedule_initial_section_control(isobus::SystemTiming::get_timestamp_ms() + INITIAL_SECTION_CONTROL_DELAY_MS); + } + if (client.second.take_due_initial_section_control(isobus::SystemTiming::get_timestamp_ms())) + { + log("TC") << "Applying initial section control state: " << (desiredSectionControlEnabled ? "enabled" : "disabled") << std::endl; + apply_section_control_state(client.first, client.second, desiredSectionControlEnabled); } } } @@ -932,20 +962,29 @@ void MyTCServer::update_section_states(std::vector §ionStates) void MyTCServer::update_section_control_enabled(bool enabled) { std::lock_guard lock(clientsMutex); + desiredSectionControlEnabled = enabled; for (auto &client : clients) { - // Always update the local flag - if (client.second.is_section_control_enabled() != enabled) - { - client.second.set_section_control_enabled(enabled); - } + apply_section_control_state(client.first, client.second, enabled); + } +} - // Only send ISOBUS command to clients that support SectionControlState DDI and have sections - if (client.second.has_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState) && - client.second.get_number_of_sections() > 0) - { - send_section_control_state(client.first, enabled); - } +void MyTCServer::set_initial_section_control_enabled(bool enabled) +{ + std::lock_guard lock(clientsMutex); + desiredSectionControlEnabled = enabled; +} + +void MyTCServer::apply_section_control_state(std::shared_ptr client, ClientState &state, bool enabled) +{ + // Always update the local flag + state.set_section_control_enabled(enabled); + + // Only send ISOBUS command to clients that support SectionControlState DDI and have sections + if (state.has_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState) && + state.get_number_of_sections() > 0) + { + send_section_control_state(client, enabled); } } From 18a6bb5f3a3fd0561d7e6bbffc4a1312d4dbddcb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 08:32:29 +0200 Subject: [PATCH 09/22] Parse each AOG UDP datagram on its own so short packets do not drop the next one The receive loop kept unparsed bytes in a static buffer between datagrams and never reset its read index after moving leftover bytes to the front. A packet shorter than 8 bytes (AOG's 7-byte section control request, PGN 241) waited in the buffer for the next datagram, and the packet after it was then read from the wrong offset, logged as "Unknown start of message" and dropped. It also read the 2 start bytes with two unsequenced index++ in one expression, and did not check that a whole packet was present before handing it on. AgIO never splits a packet across datagrams, so each datagram is now parsed on its own by aog_packet::parse_datagram (header-only, unit tested), shared by the AOG and address detection sockets. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 10 ++ include/aog_packet_parser.hpp | 78 ++++++++++++++ src/udp_connections.cpp | 169 ++++++++++--------------------- tools/test_aog_packet_parser.cpp | 155 ++++++++++++++++++++++++++++ 4 files changed, 298 insertions(+), 114 deletions(-) create mode 100644 include/aog_packet_parser.hpp create mode 100644 tools/test_aog_packet_parser.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b49f5af..a93dec6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -194,6 +194,16 @@ if(AOG_TC_VALIDATE_IOP) add_dependencies(iop_validator test_async_log) add_test(NAME async_log COMMAND test_async_log) + # AOG UDP datagram parsing; the parser is header-only. + add_executable(test_aog_packet_parser + ${CMAKE_CURRENT_LIST_DIR}/tools/test_aog_packet_parser.cpp) + target_compile_features(test_aog_packet_parser PRIVATE cxx_std_20) + set_target_properties(test_aog_packet_parser PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_aog_packet_parser + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + add_dependencies(iop_validator test_aog_packet_parser) + add_test(NAME aog_packet_parser COMMAND test_aog_packet_parser) + # PASS_REGULAR_EXPRESSION makes CTest ignore the exit code, which is the part # a CI gate actually reads. add_test(NAME malformed_pool_exit_status COMMAND iop_validator diff --git a/include/aog_packet_parser.hpp b/include/aog_packet_parser.hpp new file mode 100644 index 0000000..8e779f0 --- /dev/null +++ b/include/aog_packet_parser.hpp @@ -0,0 +1,78 @@ +/** + * @brief Splits one AgIO/AOG UDP datagram into its packets + * + * A packet is: 0x80 0x81, source, PGN, length, data bytes, CRC. + * AgIO never splits a packet across datagrams, so each datagram is parsed on its own + * and nothing is carried over to the next one. + */ + +#pragma once + +#include +#include +#include + +namespace aog_packet +{ + inline constexpr std::uint8_t START_HIGH = 0x80; + inline constexpr std::uint8_t START_LOW = 0x81; + inline constexpr std::size_t HEADER_SIZE = 5; ///< Start (2), source, PGN, length + inline constexpr std::size_t CRC_SIZE = 1; + + /// @brief Why parsing a datagram stopped before its end + enum class ParseStop + { + Complete, ///< Every byte belonged to a packet + UnknownStart, ///< Bytes at the stop offset do not start with 0x80 0x81 + Truncated ///< A packet header claims more bytes than the datagram holds + }; + + struct ParseResult + { + ParseStop stop = ParseStop::Complete; + std::size_t offset = 0; ///< Where parsing stopped; equals the datagram size when complete + std::size_t packets = 0; ///< Number of packets handed to the callback + }; + + /** + * @brief Hands every complete packet in a datagram to a callback, in order + * @param datagram The bytes of one received datagram + * @param onPacket Called as onPacket(src, pgn, data) for each packet; data excludes the CRC + * @return Where and why parsing stopped + */ + template + ParseResult parse_datagram(std::span datagram, Callback &&onPacket) + { + ParseResult result; + while (result.offset < datagram.size()) + { + const std::size_t remaining = datagram.size() - result.offset; + if (remaining < HEADER_SIZE + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + const std::uint8_t *packet = datagram.data() + result.offset; + if ((START_HIGH != packet[0]) || (START_LOW != packet[1])) + { + result.stop = ParseStop::UnknownStart; + return result; + } + + const std::uint8_t src = packet[2]; + const std::uint8_t pgn = packet[3]; + const std::size_t length = packet[4]; + if (remaining < HEADER_SIZE + length + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + onPacket(src, pgn, datagram.subspan(result.offset + HEADER_SIZE, length)); + result.offset += HEADER_SIZE + length + CRC_SIZE; + result.packets++; + } + return result; + } +} // namespace aog_packet diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index a4e7540..82e59a9 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -10,6 +10,7 @@ #include "udp_connections.hpp" #include #include +#include "aog_packet_parser.hpp" #include "logging_utils.hpp" #if !defined(_WIN32) @@ -153,15 +154,37 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) return static_cast(result & 0xFF); } +namespace +{ + void log_parse_stop(const char *socketName, std::span datagram, const aog_packet::ParseResult &result) + { + if (aog_packet::ParseStop::Complete == result.stop) + { + return; + } + + const std::size_t dropped = datagram.size() - result.offset; + if (aog_packet::ParseStop::UnknownStart == result.stop) + { + const std::uint16_t start = static_cast((datagram[result.offset] << 8) | + ((result.offset + 1 < datagram.size()) ? datagram[result.offset + 1] : 0)); + log() << "Unknown start of message: 0x" << std::hex << start << std::dec + << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + } + else + { + log() << "Truncated AOG packet (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + } + } +} + void UdpConnections::handle_incoming_packets() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -169,52 +192,14 @@ void UdpConnections::handle_incoming_packets() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + if (packetCallback) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (packetCallback) - { - packetCallback(src, pgn, { rxBuffer.data() + index, len }); - } - index += len + 1; - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; + packetCallback(src, pgn, data); } - - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; - } - } + }); + log_parse_stop("AOG", datagram, result); } else { @@ -224,13 +209,11 @@ void UdpConnections::handle_incoming_packets() void UdpConnections::handle_address_detection() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -238,71 +221,29 @@ void UdpConnections::handle_address_detection() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + // 127 is source AgIO, 201 is subnet; data is 0xC9 0xC9 IP0 IP1 IP2 + if ((0x7F != src) || (0xC9 != pgn) || (5 != data.size()) || (0xC9 != data[0]) || (0xC9 != data[1])) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (src == 0x7F && pgn == 0xC9 && len == 5 // 127 is source, AGIO, 201 is subnet - && rxBuffer[index++] == 0xC9 && rxBuffer[index++] == 0xC9) - { - // 7-8-9 is IP0,IP1,IP2 - settings->set_subnet({ rxBuffer[index++], rxBuffer[index++], rxBuffer[index++] }); - - std::cout << "Subnet from AOG: "; - std::cout << int(settings->get_subnet()[0]) << "."; - std::cout << int(settings->get_subnet()[1]) << "."; - std::cout << int(settings->get_subnet()[2]); - std::cout << " rebinding UPD connection " << std::endl; - udpConnection.close(); - udpConnection.open(udp::v4()); - udpConnection.set_option(boost::asio::socket_base::broadcast(true)); - udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); - udpConnection.non_blocking(true); - udpConnection.bind(get_local_endpoint()); - - index += len - 4; - } - else - { - index += len + 1; - } - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; + return; } - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; - } - } + settings->set_subnet({ data[2], data[3], data[4] }); + + std::cout << "Subnet from AOG: "; + std::cout << int(settings->get_subnet()[0]) << "."; + std::cout << int(settings->get_subnet()[1]) << "."; + std::cout << int(settings->get_subnet()[2]); + std::cout << " rebinding UPD connection " << std::endl; + udpConnection.close(); + udpConnection.open(udp::v4()); + udpConnection.set_option(boost::asio::socket_base::broadcast(true)); + udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); + udpConnection.non_blocking(true); + udpConnection.bind(get_local_endpoint()); + }); + log_parse_stop("address detection", datagram, result); } else { diff --git a/tools/test_aog_packet_parser.cpp b/tools/test_aog_packet_parser.cpp new file mode 100644 index 0000000..d25db51 --- /dev/null +++ b/tools/test_aog_packet_parser.cpp @@ -0,0 +1,155 @@ +/** + * @brief Unit tests for aog_packet::parse_datagram, which splits an AgIO/AOG UDP datagram into packets. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "aog_packet_parser.hpp" + +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +struct Packet +{ + std::uint8_t src; + std::uint8_t pgn; + std::vector data; +}; + +static std::vector make_packet(std::uint8_t pgn, std::vector data) +{ + std::vector bytes = { 0x80, 0x81, 0x7F, pgn, static_cast(data.size()) }; + bytes.insert(bytes.end(), data.begin(), data.end()); + bytes.push_back(0x00); // CRC, not checked + return bytes; +} + +static aog_packet::ParseResult parse(std::vector &datagram, std::vector &packets) +{ + return aog_packet::parse_datagram(datagram, [&packets](std::uint8_t src, std::uint8_t pgn, std::span data) { + packets.push_back({ src, pgn, std::vector(data.begin(), data.end()) }); + }); +} + +static void test_short_packet_is_parsed_on_its_own() +{ + std::printf("test_short_packet_is_parsed_on_its_own\n"); + + // PGN 241 (section control request) is 7 bytes; it must not wait for the next datagram. + auto datagram = make_packet(0xF1, { 0x01 }); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a 7-byte datagram parses completely"); + check(1 == packets.size(), "one packet is delivered"); + check(!packets.empty() && 0xF1 == packets[0].pgn && 1 == packets[0].data.size() && 0x01 == packets[0].data[0], "PGN and data are correct"); +} + +static void test_short_datagram_does_not_eat_the_next_one() +{ + std::printf("test_short_datagram_does_not_eat_the_next_one\n"); + + // The field failure: a 7-byte PGN 241 was kept in the buffer, the next datagram (PGN 229) was + // appended to it, and the parser then read the second packet from the wrong offset and dropped it. + auto first = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }); + std::vector packets; + parse(first, packets); + auto result = parse(second, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "the following datagram parses completely"); + check(2 == packets.size(), "both packets are delivered"); + check(2 == packets.size() && 0xE5 == packets[1].pgn && 8 == packets[1].data.size() && 0x03 == packets[1].data[0], "the PGN 229 section bits arrive intact"); +} + +static void test_several_packets_in_one_datagram() +{ + std::printf("test_several_packets_in_one_datagram\n"); + + auto datagram = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0xFF, 0x0F, 0, 0, 0, 0, 0, 0 }); + auto third = make_packet(0xF2, { 0x8D, 0x01, 1, 2, 3, 4 }); + datagram.insert(datagram.end(), second.begin(), second.end()); + datagram.insert(datagram.end(), third.begin(), third.end()); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "parses completely"); + check(3 == packets.size(), "all three packets are delivered"); + check(3 == packets.size() && 0xF1 == packets[0].pgn && 0xE5 == packets[1].pgn && 0xF2 == packets[2].pgn, "in order"); + check(3 == packets.size() && 0x0F == packets[1].data[1] && 4 == packets[2].data[5], "with their own data"); +} + +static void test_unknown_start_stops_without_delivering_garbage() +{ + std::printf("test_unknown_start_stops_without_delivering_garbage\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.insert(datagram.end(), { 0x24, 0x47, 0x50, 0x47, 0x47, 0x41, 0x2C }); // "$GPGGA," after a valid packet + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::UnknownStart == result.stop, "stops at the unknown start"); + check(14 == result.offset, "reports where it stopped"); + check(1 == packets.size(), "the valid packet before it is still delivered"); +} + +static void test_truncated_packet_is_not_delivered() +{ + std::printf("test_truncated_packet_is_not_delivered\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.resize(datagram.size() - 3); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Truncated == result.stop, "reports a truncated packet"); + check(packets.empty(), "delivers nothing past the end of the datagram"); + + std::vector headerOnly = { 0x80, 0x81, 0x7F }; + packets.clear(); + result = parse(headerOnly, packets); + check(aog_packet::ParseStop::Truncated == result.stop, "a partial header is truncated, not read past"); + check(packets.empty(), "and delivers nothing"); +} + +static void test_empty_packet() +{ + std::printf("test_empty_packet\n"); + + auto datagram = make_packet(0xC8, {}); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a zero-length packet parses"); + check(1 == packets.size() && packets[0].data.empty(), "with empty data"); +} + +int main() +{ + test_short_packet_is_parsed_on_its_own(); + test_short_datagram_does_not_eat_the_next_one(); + test_several_packets_in_one_datagram(); + test_unknown_start_stops_without_delivering_garbage(); + test_truncated_packet_is_not_delivered(); + test_empty_packet(); + + if (0 != failures) + { + std::fprintf(stderr, "%d assertion(s) failed\n", failures); + return 1; + } + std::printf("All tests passed\n"); + return 0; +} From a3652a238de3bc4d1f6530dc4475aa575c2c2f58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 08:37:24 +0200 Subject: [PATCH 10/22] Reconnect implements that missed a TC restart An implement only reconnects when it sees no TC Status for 6 s. Restarting the TC takes 2-3 s, and both our TC Status and the stack's (sent from TaskControllerServer::update()) were back on the bus before that, so the implement kept sending process data as if nothing happened. The new TC did not know it and NACKed every message; in the field a spreader stayed cut off for 20 minutes across three restarts, until the tractor was power-cycled. - Startup: hold TC Status, and the TC server update that sends the stack's copy, for 6.5 s, so clients of the previous run time out and reconnect. - Runtime: watch for Client Task and Value messages addressed to us from a CF the stack has no active client for. Such a client still thinks it is connected; hold TC Status for 6.5 s so it starts over. A 30 s cooldown lets reconnecting clients finish, and after 3 attempts for the same NAME the TC stops and asks for a power-cycle instead of disturbing the other clients. Co-Authored-By: Claude Opus 5.5 --- include/app.hpp | 16 +++++++ include/task_controller.hpp | 25 ++++++++++ src/app.cpp | 93 +++++++++++++++++++++++++++++++++++-- src/task_controller.cpp | 73 +++++++++++++++++++++++++++++ 4 files changed, 203 insertions(+), 4 deletions(-) diff --git a/include/app.hpp b/include/app.hpp index 6ee3ca7..03a0fb4 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -54,6 +54,12 @@ class Application void send_task_controller_status_message(); + /// @brief Keeps the TC off the bus (no TC Status, no TC server update) for TC_QUIET_PERIOD_MS + /// @param reason Logged, so the log shows why clients are about to reconnect + void start_tc_quiet_period(const std::string &reason); + bool is_tc_quiet(); ///< Also ends an expired quiet period + void recover_stale_clients(); + bool setup_can_hardware(); bool setup_control_functions(); void setup_task_controller_server(); @@ -100,6 +106,16 @@ class Application std::uint8_t nmea2000SequenceIdentifier = 0; std::uint32_t lastJ1939SpeedTransmit = 0; std::uint32_t lastTCStatusTransmit = 0; + + /// Longer than the 6 s a client waits for TC Status before it drops the connection and starts over + static constexpr std::uint32_t TC_QUIET_PERIOD_MS = 6500; + /// Minimum time between two quiet periods started for stale clients, so reconnecting clients get time to finish + static constexpr std::uint32_t STALE_CLIENT_RECOVERY_COOLDOWN_MS = 30000; + /// A client still stale after this many quiet periods does not reconnect this way; stop disturbing the others + static constexpr std::uint8_t MAX_STALE_CLIENT_RECOVERIES = 3; + std::uint32_t tcQuietUntilMs = 0; ///< 0 = not quiet + std::uint32_t lastStaleClientRecoveryMs = 0; + std::map staleClientRecoveries; ///< Quiet periods started per client NAME std::int32_t lastSpeedValue = 0; std::int32_t lastXteValue = 0; std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 839a4fc..b4670ea 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -18,6 +18,7 @@ #include #include #include +#include constexpr std::uint8_t NUMBER_SECTIONS_PER_CONDENSED_MESSAGE = 16; @@ -120,7 +121,25 @@ class MyTCServer : public isobus::TaskControllerServer /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated void send_gnss_quality(std::uint8_t quality); + /// @brief Starts watching for clients that talk to us as if connected although we do not know them. + /// See take_stale_clients(). Call once, after initialize(). + void start_stale_client_detection(); + + /// @brief Returns the clients that sent us a Client Task or Value message since the last call without + /// being one of our active clients. Such a client still believes it is connected (typically because the + /// TC restarted faster than the client's 6 s TC Status timeout) and the stack ignores or NACKs it until + /// it reconnects, which it will not do while it keeps seeing our TC Status. + /// Call from the thread that calls update(). + std::vector> take_stale_clients(); + + /// @brief Drops the stale client candidates collected so far, e.g. ones that arrived while the TC was quiet + void clear_stale_clients(); + + ~MyTCServer() override; + private: + static void on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer); + void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); @@ -146,4 +165,10 @@ class MyTCServer : public isobus::TaskControllerServer /// update_section_states -> send_section_setpoint_states), and get_clients() /// must keep returning a copy, not a reference (see its declaration above). mutable std::recursive_mutex clientsMutex; + + /// @brief Sources of Client Task / Value messages to us, collected on the CAN thread and checked + /// against the active clients in take_stale_clients(). Guarded by staleCandidatesMutex. + std::vector> staleCandidates; + std::mutex staleCandidatesMutex; + bool staleClientDetectionStarted = false; }; diff --git a/src/app.cpp b/src/app.cpp index ed61f32..9dfcc38 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -381,6 +381,10 @@ void Application::setup_task_controller_server() languageInterface.set_language_code(settings->get_language_code()); languageInterface.set_country_code(settings->get_country_code()); tcServer->initialize(); + tcServer->start_stale_client_detection(); + // An implement still connected to a previous run of the TC (a restart takes only a few seconds) would keep + // talking to us without reconnecting, and get NACKed, unless it sees our TC Status stop for longer than 6 s. + start_tc_quiet_period("startup"); tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG tcFunctionalities = std::make_unique(tcCF); tcFunctionalities->set_functionality_is_supported( @@ -626,9 +630,19 @@ bool Application::update() udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); - tcServer->request_measurement_commands(); - warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); - tcServer->update(); + if (is_tc_quiet()) + { + // Not calling update() also holds the stack's own TC Status; messages wait in its queue + lastTcServerUpdateCallMs = 0; + tcServer->clear_stale_clients(); + } + else + { + tcServer->request_measurement_commands(); + warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); + tcServer->update(); + recover_stale_clients(); + } if (tcFunctionalities) tcFunctionalities->update(); if (tecuFunctionalities) @@ -789,7 +803,11 @@ bool Application::update() lastLateWarningMs = isobus::SystemTiming::get_timestamp_ms(); } } - if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) + if (is_tc_quiet()) + { + lastTCStatusTransmit = 0; // Send right away when the quiet period ends, without a "late" warning + } + else if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) { static bool firstStatusSent = false; send_task_controller_status_message(); @@ -933,6 +951,73 @@ Application::ImplementDetails Application::derive_implement_details(ClientState return details; } +void Application::start_tc_quiet_period(const std::string &reason) +{ + tcQuietUntilMs = isobus::SystemTiming::get_timestamp_ms() + TC_QUIET_PERIOD_MS; + if (0 == tcQuietUntilMs) + { + tcQuietUntilMs = 1; // 0 means "not quiet" + } + log("TC") << "Holding TC Status for " << TC_QUIET_PERIOD_MS << " ms (" << reason + << ") so connected implements drop their connection and reconnect" << std::endl; +} + +bool Application::is_tc_quiet() +{ + if (0 == tcQuietUntilMs) + { + return false; + } + // Signed difference so the comparison survives timestamp wrap-around + if (static_cast(isobus::SystemTiming::get_timestamp_ms() - tcQuietUntilMs) < 0) + { + return true; + } + tcQuietUntilMs = 0; + log("TC") << "Quiet period over, sending TC Status again" << std::endl; + return false; +} + +void Application::recover_stale_clients() +{ + for (const auto &client : tcServer->take_stale_clients()) + { + const std::uint64_t name = client->get_NAME().get_full_name(); + std::ostringstream clientText; + clientText << "0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(client->get_address()) + << " (NAME " << std::dec << name << ")"; + + if ((0 != lastStaleClientRecoveryMs) && + !isobus::SystemTiming::time_expired_ms(lastStaleClientRecoveryMs, STALE_CLIENT_RECOVERY_COOLDOWN_MS)) + { + continue; // Give the clients of the last quiet period time to reconnect + } + + auto &recoveries = staleClientRecoveries[name]; + if (recoveries >= MAX_STALE_CLIENT_RECOVERIES) + { + if (MAX_STALE_CLIENT_RECOVERIES == recoveries) + { + log("TC") << "Client " << clientText.str() << " is still talking to us without being connected after " + << static_cast(MAX_STALE_CLIENT_RECOVERIES) + << " quiet periods; giving up. Power-cycle the implement to reconnect it." << std::endl; + send_hardware_message("Implement not reconnecting: power-cycle it", 20, HW_MSG_ALERT); + recoveries++; // Log once + } + continue; + } + + recoveries++; + lastStaleClientRecoveryMs = isobus::SystemTiming::get_timestamp_ms(); + log("TC") << "Client " << clientText.str() << " is talking to us as if connected, but it is not one of our clients " + << "(it missed a TC restart or timeout). Reconnecting it, attempt " << static_cast(recoveries) + << " of " << static_cast(MAX_STALE_CLIENT_RECOVERIES) << "." << std::endl; + send_hardware_message("Reconnecting implement", 5, HW_MSG_INFO); + start_tc_quiet_period("stale client " + clientText.str()); + break; // One quiet period reconnects every client + } +} + void Application::send_task_controller_status_message() { // ISO 11783-10 B.8.1 Task Controller Status message diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 614bb8a..d280f17 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -10,9 +10,11 @@ #include "logging_utils.hpp" #include "settings.hpp" +#include "isobus/isobus/can_network_manager.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" +#include #include #include #include @@ -410,6 +412,77 @@ MyTCServer::MyTCServer(std::shared_ptr internal { } +MyTCServer::~MyTCServer() +{ + if (staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + } +} + +void MyTCServer::start_stale_client_detection() +{ + if (!staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + staleClientDetectionStarted = true; + } +} + +void MyTCServer::on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer) +{ + // Runs on the CAN thread: only collect here, the active client list is checked on the update() thread + auto server = static_cast(parentPointer); + if ((nullptr == server) || + (isobus::CAN_DATA_LENGTH != message.get_data_length()) || + (nullptr == message.get_source_control_function()) || + (server->serverControlFunction != message.get_destination_control_function())) + { + return; + } + + // A client sends these only once it considers itself connected + const auto command = static_cast(message.get_data()[0] & 0x0F); + if ((ProcessDataCommands::ClientTask == command) || + (ProcessDataCommands::Value == command) || + (ProcessDataCommands::SetValueAndAcknowledge == command)) + { + std::lock_guard lock(server->staleCandidatesMutex); + if (server->staleCandidates.end() == std::find(server->staleCandidates.begin(), server->staleCandidates.end(), message.get_source_control_function())) + { + server->staleCandidates.push_back(message.get_source_control_function()); + } + } +} + +std::vector> MyTCServer::take_stale_clients() +{ + std::vector> candidates; + { + std::lock_guard lock(staleCandidatesMutex); + candidates.swap(staleCandidates); + } + + // update() has processed the stack's queue by now, so a client that just connected is already active + std::vector> staleClients; + for (const auto &candidate : candidates) + { + if (candidate->get_address_valid() && (nullptr == get_active_client(candidate))) + { + staleClients.push_back(candidate); + } + } + return staleClients; +} + +void MyTCServer::clear_stale_clients() +{ + std::lock_guard lock(staleCandidatesMutex); + staleCandidates.clear(); +} + bool MyTCServer::activate_object_pool(std::shared_ptr partnerCF, ObjectPoolActivationError &activationError, ObjectPoolErrorCodes &objectPoolError, std::uint16_t &parentObjectIDOfFaultyObject, std::uint16_t &faultyObjectID) { std::lock_guard lock(clientsMutex); From a0463b6f8969076673dbdf631eb4f8c48849807d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 09:09:40 +0200 Subject: [PATCH 11/22] Switch section control on when AOG opens a field New sectionControlOnFieldOpen setting (default on): when AOG opens a field, the TC switches section control on as if AOG's SC button was pressed (the same update_section_control_enabled(true) that PGN 241 "enabled" triggers). AOG sends the field name (PGN 0xF3) on every field open and close, and again when the TC reconnects. Older AOG versions do not, so PGN 229 starting after a 2 s gap (AOG sends it only while a field is open) counts as a field opening too. Both report the same opening, so it is acted on at most once per 5 s. Field open and close are logged. Co-Authored-By: Claude Opus 5.5 --- include/app.hpp | 10 ++++++++++ include/settings.hpp | 8 ++++++++ readme.md | 2 ++ src/app.cpp | 43 +++++++++++++++++++++++++++++++++++++++++++ src/settings.cpp | 13 +++++++++++++ 5 files changed, 76 insertions(+) diff --git a/include/app.hpp b/include/app.hpp index be6ae94..f7bed73 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -104,6 +104,16 @@ class Application std::int32_t lastXteValue = 0; std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; + + /// @brief AOG opened a field: switch section control on, as if AOG's SC button was pressed (if enabled in settings) + /// @param how What showed the field opening, for the log + void on_aog_field_opened(const std::string &how); + /// AOG sends PGN 229 every frame while a field is open; a gap this long means no field was open + static constexpr std::uint32_t FIELD_CLOSED_GAP_MS = 2000; + /// The field name (PGN 0xF3) and PGN 229 resuming both report one field opening; act on it once + static constexpr std::uint32_t FIELD_OPEN_DEDUPLICATE_MS = 5000; + std::uint32_t lastSectionStatesMs = 0; ///< When PGN 229 was last received, 0 = never + std::uint32_t lastFieldOpenAppliedMs = 0; ///< When on_aog_field_opened() last switched section control on, 0 = never std::uint32_t vtDisconnectedSinceMs = 0; std::uint32_t lastVtStatusUpdateMs = 0; std::uint32_t lastVtSectionUpdateMs = 0; diff --git a/include/settings.hpp b/include/settings.hpp index 0373b55..d66fe4c 100644 --- a/include/settings.hpp +++ b/include/settings.hpp @@ -104,6 +104,12 @@ class Settings /// @brief Set whether section control is switched on automatically when an implement connects. bool set_section_control_on_connect(bool enabled, bool save = true); + /// @brief Check whether section control is switched on when AOG opens a field, as if AOG's SC button was pressed. + bool is_section_control_on_field_open() const; + + /// @brief Set whether section control is switched on when AOG opens a field. + bool set_section_control_on_field_open(bool enabled, bool save = true); + /** * @brief Get the configured TC version * @return The configured version (0-4, default 3) @@ -168,6 +174,7 @@ class Settings constexpr static bool DEFAULT_AOG_HEARTBEAT_ENABLED = true; constexpr static bool DEFAULT_VT_ENABLED = true; constexpr static bool DEFAULT_SECTION_CONTROL_ON_CONNECT = true; + constexpr static bool DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN = true; constexpr static std::uint8_t DEFAULT_TC_VERSION = 3; ///< SecondEditionDraft (V3 default for maximum implement compatibility) static const std::string DEFAULT_LANGUAGE_CODE; static const std::string DEFAULT_COUNTRY_CODE; @@ -177,6 +184,7 @@ class Settings bool aogHeartbeatEnabled = DEFAULT_AOG_HEARTBEAT_ENABLED; bool vtEnabled = DEFAULT_VT_ENABLED; bool sectionControlOnConnect = DEFAULT_SECTION_CONTROL_ON_CONNECT; + bool sectionControlOnFieldOpen = DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN; std::uint8_t tcVersion = DEFAULT_TC_VERSION; std::string languageCode = DEFAULT_LANGUAGE_CODE; std::string countryCode = DEFAULT_COUNTRY_CODE; diff --git a/readme.md b/readme.md index 997f0a3..63fed97 100644 --- a/readme.md +++ b/readme.md @@ -59,6 +59,8 @@ AOG-TaskController reads its configuration from a `settings.json` file located i | `nmeaSendEnabled` | `bool` | `true` | Enables cyclic NMEA2000 COG/SOG transmission. Requires `tecuEnabled`. | | `aogHeartbeatEnabled` | `bool` | `true` | Sends the 100 ms zero-section heartbeat when no implement is connected. Disable for AgOpenGPS versions before v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Enables the ISOBUS Virtual Terminal UI. | +| `sectionControlOnConnect` | `bool` | `true` | Switches section control on 2 s after an implement connects, as if AOG's SC button was pressed. | +| `sectionControlOnFieldOpen` | `bool` | `true` | Switches section control on when AOG opens a field (field name, PGN 0xF3, or section states, PGN 229, starting after a gap), as if AOG's SC button was pressed. | | `tcVersion` | `integer` | `3` | Task Controller version code, from `0` through `4`; `3` is Second Edition Draft. | | `languageCode` | `string` | `"en"` | Two-character language code advertised on ISOBUS. | | `countryCode` | `string` | `"US"` | Two-character country code advertised on ISOBUS. | diff --git a/src/app.cpp b/src/app.cpp index fcff617..2600a9b 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -447,6 +447,27 @@ void Application::setup_tecu_interfaces() } } +void Application::on_aog_field_opened(const std::string &how) +{ + if (!settings->is_section_control_on_field_open()) + { + return; + } + if ((0 != lastFieldOpenAppliedMs) && !isobus::SystemTiming::time_expired_ms(lastFieldOpenAppliedMs, FIELD_OPEN_DEDUPLICATE_MS)) + { + return; + } + lastFieldOpenAppliedMs = isobus::SystemTiming::get_timestamp_ms(); + if (0 == lastFieldOpenAppliedMs) + { + lastFieldOpenAppliedMs = 1; // 0 means "never" + } + + // Saves pressing AOG's SC button after every field change; same effect as PGN 241 "enabled" + log("TC") << "AOG " << how << ": switching section control on, as if AOG's SC button was pressed" << std::endl; + tcServer->update_section_control_enabled(true); +} + void Application::setup_udp_connections() { static std::uint8_t xteSid = 0; @@ -470,6 +491,28 @@ void Application::setup_udp_connections() } } tcServer->update_section_states(sectionStates); + + // Older AOG versions do not send the field name, but PGN 229 only flows while a field is open + const bool resumed = (0 == lastSectionStatesMs) || isobus::SystemTiming::time_expired_ms(lastSectionStatesMs, FIELD_CLOSED_GAP_MS); + lastSectionStatesMs = (0 == lastAogPacketMs) ? 1 : lastAogPacketMs; + if (resumed) + { + on_aog_field_opened("started sending section states (PGN 229)"); + } + } + else if (pgn == 0xF3) // 243 - Field name, empty when the field is closed + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + const std::string fieldName(data.begin(), data.end()); + if (fieldName.empty()) + { + log("Field") << "AOG closed the field" << std::endl; + } + else + { + log("Field") << "AOG opened field '" << fieldName << "'" << std::endl; + on_aog_field_opened("opened field '" + fieldName + "'"); + } } else if (pgn == 0xF1 && !data.empty()) // 241 - Section Control { diff --git a/src/settings.cpp b/src/settings.cpp index 8b6c53f..7925979 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -71,6 +71,7 @@ bool Settings::load() aogHeartbeatEnabled = loadBoolean("aogHeartbeatEnabled", DEFAULT_AOG_HEARTBEAT_ENABLED); vtEnabled = loadBoolean("vtEnabled", DEFAULT_VT_ENABLED); sectionControlOnConnect = loadBoolean("sectionControlOnConnect", DEFAULT_SECTION_CONTROL_ON_CONNECT); + sectionControlOnFieldOpen = loadBoolean("sectionControlOnFieldOpen", DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN); if (data.contains("tcVersion")) { @@ -145,6 +146,7 @@ bool Settings::save() const data["aogHeartbeatEnabled"] = aogHeartbeatEnabled; data["vtEnabled"] = vtEnabled; data["sectionControlOnConnect"] = sectionControlOnConnect; + data["sectionControlOnFieldOpen"] = sectionControlOnFieldOpen; data["tcVersion"] = tcVersion; data["languageCode"] = languageCode; data["countryCode"] = countryCode; @@ -274,6 +276,17 @@ bool Settings::set_section_control_on_connect(bool enabled, bool save) return set_boolean(sectionControlOnConnect, enabled, save); } +bool Settings::is_section_control_on_field_open() const +{ + std::scoped_lock lock(settingsMutex); + return sectionControlOnFieldOpen; +} + +bool Settings::set_section_control_on_field_open(bool enabled, bool save) +{ + return set_boolean(sectionControlOnFieldOpen, enabled, save); +} + bool Settings::is_aog_heartbeat_enabled() const { std::scoped_lock lock(settingsMutex); From cbf1e59f22e602f9256a30221f3cca43bd28a52d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Wed, 14 Oct 2026 18:02:21 +0200 Subject: [PATCH 12/22] Log the sender and bytes of datagrams that are not AOG packets The field log showed datagrams with 2 or 4 bytes after a complete packet, and others that are not AOG packets at all (one starts with "$P"). The log only said how many bytes were dropped, so the sender could not be identified. Log the sender's address and port, and up to 64 bytes as hex with a "|" where parsing stopped, followed by the printable characters. Co-Authored-By: Claude Opus 5.5 --- src/udp_connections.cpp | 46 ++++++++++++++++++++++++++++------------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index 82e59a9..75b5651 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -8,8 +8,11 @@ */ #include "udp_connections.hpp" +#include #include +#include #include +#include #include "aog_packet_parser.hpp" #include "logging_utils.hpp" @@ -156,25 +159,40 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) namespace { - void log_parse_stop(const char *socketName, std::span datagram, const aog_packet::ParseResult &result) + /// Bytes of a datagram shown in the log when it is not (only) AOG packets + constexpr std::size_t MAX_LOGGED_DATAGRAM_BYTES = 64; + + /// @brief Hex dump with a "|" where parsing stopped, then the printable characters (others as ".") + std::string describe_datagram(std::span datagram, std::size_t stopOffset) { - if (aog_packet::ParseStop::Complete == result.stop) + const std::size_t count = std::min(datagram.size(), MAX_LOGGED_DATAGRAM_BYTES); + std::ostringstream hex; + std::string text; + hex << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < count; i++) { - return; + hex << ((i == stopOffset) ? ((0 == i) ? "| " : " | ") : ((0 == i) ? "" : " ")) << std::setw(2) << static_cast(datagram[i]); + text += ((datagram[i] >= 0x20) && (datagram[i] < 0x7F)) ? static_cast(datagram[i]) : '.'; } - - const std::size_t dropped = datagram.size() - result.offset; - if (aog_packet::ParseStop::UnknownStart == result.stop) + if (count < datagram.size()) { - const std::uint16_t start = static_cast((datagram[result.offset] << 8) | - ((result.offset + 1 < datagram.size()) ? datagram[result.offset + 1] : 0)); - log() << "Unknown start of message: 0x" << std::hex << start << std::dec - << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + hex << " ..."; } - else + return hex.str() + " \"" + text + "\""; + } + + void log_parse_stop(const char *socketName, const udp::endpoint &sender, std::span datagram, const aog_packet::ParseResult &result) + { + if (aog_packet::ParseStop::Complete == result.stop) { - log() << "Truncated AOG packet (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes)" << std::endl; + return; } + + const std::size_t dropped = datagram.size() - result.offset; + log() << ((aog_packet::ParseStop::UnknownStart == result.stop) ? "Unknown start of message" : "Truncated AOG packet") + << " from " << sender.address().to_string() << ":" << sender.port() + << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes): " + << describe_datagram(datagram, result.offset) << std::endl; } } @@ -199,7 +217,7 @@ void UdpConnections::handle_incoming_packets() packetCallback(src, pgn, data); } }); - log_parse_stop("AOG", datagram, result); + log_parse_stop("AOG", sender_endpoint, datagram, result); } else { @@ -243,7 +261,7 @@ void UdpConnections::handle_address_detection() udpConnection.non_blocking(true); udpConnection.bind(get_local_endpoint()); }); - log_parse_stop("address detection", datagram, result); + log_parse_stop("address detection", sender_endpoint, datagram, result); } else { From 6e1f83ee39156fc05626b7b2ef8f9c35656a9de0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 15:51:25 +0100 Subject: [PATCH 13/22] Run the TECU as a secondary Tractor ECU when the tractor has one ISO 11783-9:2012 4.4.2.1: with several Tractor ECUs on the implement bus, function instance 0 is the primary, and a higher instance must not send what a lower one provides, requests PGN 65033 from it, and sets only the facility bits it lacks. - At startup, claim 0xF0 as the primary only if no other Tractor ECU answered the TC's address claim. Otherwise claim an arbitrary address as a secondary with the next function instance. - The TECU NAME is now arbitrary-address-capable, its most significant bit, so a primary loses a contest for 0xF0 to a tractor's fixed-address Tractor ECU instead of taking the address from it. - Once a second, check for other Tractor ECUs. A primary steps down when another instance 0 appears or it lost 0xF0, rebuilding the control function without blocking the main loop. - A secondary sends nothing until the lower instances answered PGN 65033 (2.5 s at most), then sends each of ground-based, wheel-based and machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG only if none of them advertises it or sent it in the last 3 s. PGN 65033 advertises exactly what it sends, and is re-sent on change. - The primary now advertises the selected speed facility (byte 5 bit 3), since the TECU sends machine selected speed (PGN 61474). - TractorFacilities gets a destructor that removes its callbacks, and set_provided_facilities(). Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 16 ++ docs/CONCURRENCY.md | 13 ++ docs/PROTOCOL.md | 27 ++- include/app.hpp | 41 ++++ include/tecu_coexistence.hpp | 151 +++++++++++++ include/tractor_facilities.hpp | 18 ++ src/app.cpp | 364 +++++++++++++++++++++++++++----- src/tecu_coexistence.cpp | 292 +++++++++++++++++++++++++ src/tractor_facilities.cpp | 35 +++ tools/test_tecu_coexistence.cpp | 157 ++++++++++++++ 10 files changed, 1050 insertions(+), 64 deletions(-) create mode 100644 include/tecu_coexistence.hpp create mode 100644 src/tecu_coexistence.cpp create mode 100644 tools/test_tecu_coexistence.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ef926f..482e0c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -227,6 +227,22 @@ if(AOG_TC_VALIDATE_IOP) isobus::Utility) add_test(NAME tractor_facilities_encode_decode COMMAND test_tractor_facilities) + + # Sharing the bus with other Tractor ECUs (ISO 11783-9 4.4.2.1) + add_executable( + test_tecu_coexistence + ${CMAKE_CURRENT_LIST_DIR}/tools/test_tecu_coexistence.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/tecu_coexistence.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/tractor_facilities.cpp) + target_compile_features(test_tecu_coexistence PRIVATE cxx_std_20) + set_target_properties(test_tecu_coexistence PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_tecu_coexistence + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + target_link_libraries( + test_tecu_coexistence PRIVATE isobus::Isobus isobus::HardwareIntegration + isobus::Utility) + add_dependencies(iop_validator test_tecu_coexistence) + add_test(NAME tecu_coexistence COMMAND test_tecu_coexistence) endif() if(WIN32) diff --git a/docs/CONCURRENCY.md b/docs/CONCURRENCY.md index 3d6cd47..dcbbb3e 100644 --- a/docs/CONCURRENCY.md +++ b/docs/CONCURRENCY.md @@ -59,6 +59,19 @@ registration point itself defers to an `update()` call, or invokes the listener/ overrides may turn out to already be main-thread-only — the lock is cheap insurance and keeps every entry point consistent regardless of how the library's internals might change. +- **`OtherTecuMonitor`** (`tecu_coexistence.hpp`/`.cpp`) — `on_frame()` (frame-received event) and its + PGN 65033 callback run on the background thread, `refresh()` and the queries on the main thread. The + per-address and per-message flags are atomics; the facility map is under its own mutex. It lives as long + as `Application` because the stack calls global PGN callbacks from a copy of the list, so one can still run + once after `remove_global_parameter_group_number_callback()` returns. +- **Rebuilding the TECU at runtime** (`Application::teardown_tecu()`, when stepping down to a secondary Tractor + ECU) destroys `TractorFacilities`, `SpeedMessagesInterface`, `NMEA2000MessageInterface` and the functionalities + object before `deactivate_control_function()`. The PGN request protocol and the "any control function" + callbacks are invoked under the same locks their removal takes, so they cannot run against a destroyed + object. `TractorFacilities`' PGN 65032 callback may still run once after removal, which is harmless: it only + logs and never touches the object. `TractorFacilities::providedFacilities` is under its own mutex + (set on the main thread, read when answering a PGN 65033 request). + ## What's *not* protected yet — known gap Calls into `vtClient` (`isobus::VirtualTerminalClient`) happen from both the main thread diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index f396397..054e29f 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -257,7 +257,7 @@ Two control functions, both claiming addresses via standard J1939-81 address cla | CF | NAME function | Address | Notes | |---|---|---|---| | Task Controller | `TaskController` (function code 61) | Preferred `247` (ISO 11783-10 MappingComputer). Walks if claimed. | Always present. | -| Tractor ECU | `TractorECU` (function code 132) | Fixed `240` (non-arbitrary-address-capable per ISO 11783-9). | Only present if `tecuEnabled: true`. | +| Tractor ECU | `TractorECU` (function code 134) | `240` as the primary; an arbitrary address (preferred `128`) as a secondary, see §5.7. | Only present if `tecuEnabled: true`. | Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufacturer Code `1407`, Identity `20`. Override these in `app.cpp` if you fork. @@ -267,10 +267,11 @@ 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. | +| `0xFEE8` (PGN 65256 Speed/Direction) | 100 ms | TECU | Ground/Wheel/Machine-selected speed + machine direction, J1939 format. Only when TECU enabled, and not while another Tractor ECU sends it (§5.7). | | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). | -| `0xFE09` (PGN 65033 Tractor Facilities) | Power-up + on request | TECU | 8-byte facility bitmask advertising which PGNs the TECU actually broadcasts. See §5.6. | -| NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. | +| `0xFE48`, `0xFE49`, `0xF022` (PGN 65096, 65097, 61474) | 100 ms | TECU | Wheel-based speed, ground-based speed, machine selected speed (with distance and direction). As a secondary, only those no other Tractor ECU provides (§5.7). | +| `0xFE09` (PGN 65033 Tractor Facilities) | Power-up + on request, and when it changes | TECU | 8-byte facility bitmask advertising which PGNs the TECU actually broadcasts. See §5.6. | +| NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. Not while another Tractor ECU sends it (§5.7). | | GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | GPS fix quality from AgIO (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` if `0xD6` has never arrived, and to `0` once it stops arriving (see §2.5). | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. @@ -310,10 +311,11 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 | Byte | Bit(s) | Facility | Condition | |---|---|---|---| | 1 | 8,7 | TECU class | Always `00` (Class 1). | -| 1 | 2 | Ground-based speed (PGN 65097) | `speedMessagesInterface` exists (always true when TECU is enabled). | -| 1 | 3 | Wheel-based speed (PGN 65096) | `speedMessagesInterface` exists (always true when TECU is enabled). | +| 1 | 2 | Ground-based speed (PGN 65097) | The TECU sends it: always as the primary; as a secondary only if no other Tractor ECU provides it (§5.7). | +| 1 | 3 | Wheel-based speed (PGN 65096) | Same rule. | | 3 | 7,6 | Ground-based distance + direction | Same as ground-based speed. | | 3 | 5,4 | Wheel-based distance + direction | Same as wheel-based speed. | +| 5 | 3 | Selected speed (machine selected speed, PGN 61474) | Same rule. | **Facilities NOT advertised (bits always 0):** @@ -325,15 +327,24 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 - Lighting — no lighting controller. - Language command storage (PGN 65039) — not broadcast by the TECU. - Auxiliary valve commands / status — no valve interface. -- Selected speed (PGN 65265) — not broadcast. - Navigation position data / high-output position — NMEA 2000 position PGNs are not forwarded over Fast Packet. - Front hitch / PTO — no front hitch or PTO sensors. - All reserved bits (byte 2 bits 2–1, byte 4 bits 3–1, byte 5 bit 5, byte 7, byte 8 including the reserved-bit indicator at bit 1). -**Default payload** (TECU enabled, speed broadcasts active): `[0x06, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00]`. +**Default payload** (TECU enabled as the primary): `[0x06, 0x00, 0x78, 0x00, 0x04, 0x00, 0x00, 0x00]`. **PGN 65032 (Required Tractor Facilities):** When an implement broadcasts what it needs, the TC logs the request at debug level. The response is not modified based on the implement's requirements — a facility bit is set to 1 only when backed by a live broadcast. +### 5.7 Sharing the bus with a tractor's Tractor ECU + +ISO 11783-9:2012 §4.4.2.1 allows several Tractor ECUs on the implement bus. The one with function instance 0 is the primary; one with a higher instance "shall not provide any messages which are provided by a Tractor ECU with a lower function instance", requests PGN 65033 from those, and sets only the facility bits they lack (the standard's example is a display adding ground-based speed from GNSS to a tractor without radar). + +- **At startup** the TC's own address claim makes every control function on the bus announce itself. If no other Tractor ECU (function code 134) is there, our TECU claims `240` as the primary (function instance 0) and sends everything in §5.2. If one is there, it claims an arbitrary address as a secondary, with the next function instance. +- **Never taking `240` from a tractor:** our TECU NAME is arbitrary-address-capable in both roles. That is the most significant NAME bit, so in an address claim contest for `240` our primary loses to a fixed-address Tractor ECU and moves elsewhere, instead of knocking the tractor's ECU off the bus. +- **Stepping down at runtime:** once a second the TC checks for other Tractor ECUs. If another one with function instance 0 appears while ours is the primary, or ours lost `240`, our TECU is rebuilt as a secondary above every Tractor ECU's function instance. Another secondary appearing is no reason to step down. The operator sees a hardware message ("Tractor ECU found: AOG adds only missing data"). +- **What a secondary sends:** nothing until the other Tractor ECUs have answered PGN 65033 (and at least 1 s has passed) or 2.5 s have passed. Then each of ground-based speed, wheel-based speed, machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG is sent only if no Tractor ECU with a lower function instance advertises it in PGN 65033 *and* none has sent it in the last 3 s. Only lower instances count, so two secondaries never wait on each other. Its PGN 65033 advertises exactly what it sends, and is re-sent when that changes. +- A secondary does not step back up to primary if the tractor's Tractor ECU leaves; it then sends everything, still as a secondary. + --- ## 6. Example clients diff --git a/include/app.hpp b/include/app.hpp index 00de4a4..8475800 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -25,6 +26,7 @@ #include "logging_utils.hpp" #include "settings.hpp" #include "task_controller.hpp" +#include "tecu_coexistence.hpp" #include "tractor_facilities.hpp" #include "udp_connections.hpp" @@ -72,6 +74,24 @@ class Application bool setup_control_functions(); void setup_task_controller_server(); void setup_tecu_interfaces(); + + /// Our TECU's place among the Tractor ECUs on the bus (ISO 11783-9 4.4.2.1) + enum class TecuRole + { + None, ///< No TECU (disabled, or it could not claim an address) + Primary, ///< Function instance 0 at address 0xF0: sends everything + Secondary ///< Another Tractor ECU is on the bus: we only add what it lacks + }; + /// @brief Create the TECU control function and start its address claim (does not wait for it) + void start_tecu_claim(TecuRole role, std::uint8_t functionInstance); + /// @brief Finishes a pending claim, and once a second checks for other Tractor ECUs: + /// steps down from primary when one appears, and as a secondary sends only what they lack. + void update_tecu(); + /// @brief Destroy the TECU interfaces and control function, so it can be claimed again with another NAME + void teardown_tecu(); + /// @brief Start or stop our TECU messages to match the plan, and advertise them in PGN 65033 + void apply_tecu_plan(const TecuMessagePlan &plan); + void create_speed_messages_interface(const TecuMessagePlan &plan); void setup_udp_connections(); void setup_vt_client(); @@ -124,6 +144,27 @@ class Application bool tractorFacilitiesSentOnPowerUp = false; + static constexpr std::uint32_t TECU_CLAIM_TIMEOUT_MS = 5000; + static constexpr std::uint32_t TECU_CHECK_INTERVAL_MS = 1000; + /// As a secondary, send nothing until the other TECUs answered PGN 65033, or this long has passed + static constexpr std::uint32_t TECU_PLAN_SETTLE_MS = 2500; + /// ...but always watch the bus at least this long, for messages they send without advertising + static constexpr std::uint32_t TECU_PLAN_MIN_SETTLE_MS = 1000; + static constexpr std::uint8_t SECONDARY_TECU_PREFERRED_ADDRESS = 0x80; ///< Arbitrary range; never 0xF0 + static constexpr std::uint8_t ALL_FUNCTION_INSTANCES = 31; ///< Highest NAME function instance (5 bits) + isobus::NAME tecuNameTemplate{ 0 }; ///< Our NAME fields, set in setup_control_functions() + OtherTecuMonitor otherTecus; + TecuRole tecuRole = TecuRole::None; + std::uint8_t tecuFunctionInstance = 0; + std::uint32_t tecuClaimStartedMs = 0; + std::uint32_t tecuClaimedMs = 0; ///< 0 while the claim is pending + bool tecuInterfacesReady = false; + std::uint32_t tecuInterfacesReadyMs = 0; + std::uint32_t lastTecuCheckMs = 0; + bool tecuPlanSettled = false; + TecuMessagePlan tecuPlan; ///< What our TECU sends now + std::set knownOtherTecuAddresses; ///< To log other TECUs appearing and leaving + static constexpr std::uint32_t SECTION_DEMAND_LOG_INTERVAL_MS = 200; static constexpr std::uint32_t SECTION_DEMAND_TIMEOUT_MS = 2000; ///< AOG sends PGN 229 every frame while a field is open static constexpr std::uint32_t WORKING_WITHOUT_DEMAND_WARNING_MS = 10000; diff --git a/include/tecu_coexistence.hpp b/include/tecu_coexistence.hpp new file mode 100644 index 0000000..99fcacb --- /dev/null +++ b/include/tecu_coexistence.hpp @@ -0,0 +1,151 @@ +/** + * @brief Sharing the implement bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1). + * + * When several Tractor ECUs are on the implement bus, the one with function + * instance 0 is the primary. A Tractor ECU with a higher function instance + * "shall not provide any messages which are provided by a Tractor ECU with a + * lower function instance", requests the tractor facility response (PGN 65033) + * from those, and "shall only set those facilities bit(s) that are not available + * in the other Tractor ECUs". + * + * Our TECU runs as the primary when it is alone, and as a secondary otherwise. + * This file holds the decision of what a secondary sends (pure functions, unit + * tested) and OtherTecuMonitor, which collects what the other TECUs advertise + * and actually send. + */ + +#pragma once + +#include "tractor_facilities.hpp" + +#include "isobus/isobus/can_control_function.hpp" +#include "isobus/isobus/can_message.hpp" +#include "isobus/isobus/can_message_frame.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// Messages our TECU can put on the bus +enum class TecuMessage : std::uint8_t +{ + GroundBasedSpeed, ///< PGN 65097, ground-based speed, distance and direction + WheelBasedSpeed, ///< PGN 65096, wheel-based speed, distance and direction + MachineSelectedSpeed, ///< PGN 61474 + VehicleDirectionSpeed, ///< PGN 65256, J1939 vehicle direction/speed (no facility bit) + CogSogRapidUpdate, ///< PGN 129026, NMEA 2000 COG & SOG rapid update (no facility bit) + TimeDate, ///< PGN 65254 + Count +}; + +constexpr std::size_t TECU_MESSAGE_COUNT = static_cast(TecuMessage::Count); + +/// @brief The PGN a TecuMessage is sent with +std::uint32_t tecu_message_pgn(TecuMessage message); + +/// @brief Short name for logs, e.g. "ground-based speed (PGN 65097)" +std::string tecu_message_name(TecuMessage message); + +/// Which of our messages to send +struct TecuMessagePlan +{ + std::array send{}; + + bool sends(TecuMessage message) const + { + return send[static_cast(message)]; + } + bool operator==(const TecuMessagePlan &other) const + { + return send == other.send; + } + bool operator!=(const TecuMessagePlan &other) const + { + return send != other.send; + } +}; + +/// @brief Decide which messages our TECU sends. +/// @param isPrimary True when our TECU is the primary (function instance 0): it sends everything. +/// @param advertisedByOthers Facilities the other (lower instance) TECUs report in PGN 65033, combined. +/// @param sentByOthers Messages seen recently from another TECU, whether advertised or not. +/// @returns As a secondary, only the messages no other TECU provides (ISO 11783-9 4.4.2.1 c). +TecuMessagePlan plan_tecu_messages(bool isPrimary, + const Facilities &advertisedByOthers, + const std::array &sentByOthers); + +/// @brief The facilities to advertise for a plan (ISO 11783-9 4.4.2.1 d): exactly the messages we send. +/// The time/date bit is left clear; it is set separately while time/date is actually provided. +Facilities facilities_for_plan(const TecuMessagePlan &plan); + +/// @brief Bitwise OR of two facility sets (what any of several TECUs provides) +Facilities merge_facilities(const Facilities &a, const Facilities &b); + +/// @brief The J1939 PGN of a 29-bit CAN identifier (PDU1 destination address removed) +std::uint32_t pgn_from_can_identifier(std::uint32_t identifier); + +/// Another Tractor ECU on the bus +struct OtherTecu +{ + std::shared_ptr controlFunction; + std::uint8_t address = 0xFE; + std::uint8_t functionInstance = 0; + std::uint16_t manufacturerCode = 0; +}; + +/// @brief Tracks the other Tractor ECUs: who they are, the facilities they advertise and what they send. +/// +/// refresh() and the queries run on the main thread. on_frame() and the PGN 65033 callback run on the +/// CAN stack's background thread (see docs/CONCURRENCY.md), so the shared state is atomic or under a mutex. +/// The object lives as long as the Application: the stack may still call a global PGN callback once +/// after it was removed, so this must never be destroyed while the stack runs. +class OtherTecuMonitor +{ +public: + /// How long a message seen from another TECU counts as "sent by it" + static constexpr std::uint32_t SENT_BY_OTHERS_WINDOW_MS = 3000; + + /// @brief Register the PGN 65033 listener. Call once. + void initialize(); + + /// @brief Re-read the other TECUs from the network manager, excluding our own control functions. + /// Only TECUs with a function instance up to maxFunctionInstance count: a secondary defers to lower + /// instances only (ISO 11783-9 4.4.2.1 c), so two secondaries never wait on each other. Forgets + /// facilities of TECUs that have gone. + /// @returns Those TECUs currently on the bus, lowest function instance first. + std::vector refresh(std::uint8_t maxFunctionInstance); + + /// @brief Called for every received CAN frame (background thread) + void on_frame(const isobus::CANMessageFrame &frame); + + /// @brief The facilities advertised by the other TECUs, combined + Facilities advertised_facilities() const; + + /// @brief Whether every other TECU has answered with PGN 65033 + bool all_facilities_received() const; + + /// @brief Messages seen from another TECU within SENT_BY_OTHERS_WINDOW_MS + std::array sent_by_others() const; + + /// @brief Whether we already asked this address for PGN 65033 since it appeared + bool facilities_requested(std::uint8_t address) const; + void mark_facilities_requested(std::uint8_t address); + +private: + static void on_facilities_response(const isobus::CANMessage &message, void *parent); + + std::array, 256> isOtherTecuAddress{}; + std::array, TECU_MESSAGE_COUNT> lastSentByOthersMs{}; ///< 0 = never + + mutable std::mutex mutex; ///< Guards the members below + std::map facilitiesByAddress; + std::set otherAddresses; + std::set requestedAddresses; + std::map nameByAddress; ///< To notice a different TECU taking over an address +}; diff --git a/include/tractor_facilities.hpp b/include/tractor_facilities.hpp index 6173cc6..28f556d 100644 --- a/include/tractor_facilities.hpp +++ b/include/tractor_facilities.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include class Settings; @@ -104,6 +105,12 @@ class TractorFacilities TractorFacilities(std::shared_ptr tecuCF, std::shared_ptr settings); + /// @brief Removes the callbacks registered by initialize(), so the TECU can be rebuilt at runtime + ~TractorFacilities(); + + TractorFacilities(const TractorFacilities &) = delete; + TractorFacilities &operator=(const TractorFacilities &) = delete; + /// @brief Register the PGN 65033 request callback and the PGN 65032 /// global receive listener. Must be called after the TECU /// address claim has completed. @@ -126,6 +133,11 @@ class TractorFacilities /// When true, the time/date facility bit is advertised in PGN 65033. void set_time_date_active(bool active); + /// @brief Advertise exactly these facilities (the time/date bit still follows set_time_date_active()). + /// Used when our TECU shares the bus with other Tractor ECUs and only provides what they lack + /// (ISO 11783-9 4.4.2.1). Until this is called, the facilities follow the speed interface. + void set_provided_facilities(const Facilities &facilities); + /// @brief Build the 8-byte payload from the current runtime state. std::array build_payload() const; @@ -152,6 +164,12 @@ class TractorFacilities /// concurrently on different threads. std::atomic timeDateActive{ false }; + /// Set by set_provided_facilities() on the main thread, read by build_payload() from PGN request callbacks + mutable std::mutex providedFacilitiesMutex; + bool hasProvidedFacilities = false; + Facilities providedFacilities; + bool initialized = false; + /// Source addresses for which we have already logged an info-level /// "request received" line, to avoid per-message spam. std::set loggedRequesters; diff --git a/src/app.cpp b/src/app.cpp index e1eb8e7..5f2f334 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -290,10 +290,9 @@ bool Application::setup_control_functions() isobus::NAME tcNAME = ourNAME; tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); - isobus::NAME tecuNAME = ourNAME; - tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); - tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed - tecuNAME.set_ecu_instance(0); + tecuNameTemplate = ourNAME; + tecuNameTemplate.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); + tecuNameTemplate.set_ecu_instance(0); log("Init") << "Creating Task Controller control function..." << std::endl; tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 @@ -342,62 +341,62 @@ bool Application::setup_control_functions() } } - // Create TECU control function - // TODO: Should we wait between this and TC? - // TODO: If there's already a TECU on the bus we should not create ours + // Create TECU control function. By now the TC's own address claim made every CF on the bus + // announce itself, so a tractor's Tractor ECU is already known (ISO 11783-9 4.4.2.1). if (tcCF && settings->is_tecu_enabled()) { // Only create TECU if TC was created and ECU is enabled - log("Init") << "Creating Tractor ECU control function..." << std::endl; - tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tecuNAME, 0, isobus::preferred_addresses::IndustryGroup2::TractorECU); + otherTecus.initialize(); + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener([this](const isobus::CANMessageFrame &frame) { + otherTecus.on_frame(frame); // CAN thread: notes what the other TECUs send + }); - // Wait for TECU address claim with minimum 250ms delay per J1939-81 section 4.4.4.1 - log("Init") << "Tractor ECU control function created, waiting for address claim..." << std::endl; + const auto others = otherTecus.refresh(ALL_FUNCTION_INSTANCES); + if (others.empty()) + { + start_tecu_claim(TecuRole::Primary, 0); + } + else + { + for (const auto &other : others) + { + knownOtherTecuAddresses.insert(other.address); + log("Init") << "Tractor ECU already on the bus at " << format_hex_address(other.address) << " (function instance " + << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + } + start_tecu_claim(TecuRole::Secondary, static_cast(std::min(others.back().functionInstance + 1, ALL_FUNCTION_INSTANCES))); + } - // Update the network manager to process TECU CF claiming - int tecuClaimAttempts = 0; - const int MAX_TECU_CLAIM_ATTEMPTS = 50; // 5 seconds max - while (!tecuCF->get_address_valid() && tecuClaimAttempts < MAX_TECU_CLAIM_ATTEMPTS) + // Wait for the claim here at startup, as before, so the TECU interfaces exist once setup is done. + // At runtime update_tecu() finishes claims without blocking. + while (!tecuCF->get_address_valid() && !isobus::SystemTiming::time_expired_ms(tecuClaimStartedMs, TECU_CLAIM_TIMEOUT_MS)) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); isobus::CANNetworkManager::CANNetwork.update(); - tecuClaimAttempts++; } - // TECU is non-arbitrary-address-capable: it must land on its preferred address or not at all - if (tecuCF->get_address_valid() && tecuCF->get_address() == isobus::preferred_addresses::IndustryGroup2::TractorECU) + if (tecuCF->get_address_valid()) { // Record when the address was actually claimed for the 250ms delay calculation - auto tecuAddressClaimedTime = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = (0 == tecuClaimedMs) ? 1 : tecuClaimedMs; log("Init") << "TECU claimed address " << static_cast(tecuCF->get_address()) << std::endl; // Ensure minimum 250ms delay after address claim per J1939-81 - auto tecuClaimElapsedMs = isobus::SystemTiming::get_time_elapsed_ms(tecuAddressClaimedTime); - if (tecuClaimElapsedMs < MINIMUM_ADDRESS_CLAIM_DELAY_MS) + log("Init") << "Waiting " << MINIMUM_ADDRESS_CLAIM_DELAY_MS << "ms after TECU address claim (J1939-81 250ms rule)..." << std::endl; + auto delayStart = isobus::SystemTiming::get_timestamp_ms(); + while (isobus::SystemTiming::get_time_elapsed_ms(delayStart) < MINIMUM_ADDRESS_CLAIM_DELAY_MS) { - auto remainingDelay = MINIMUM_ADDRESS_CLAIM_DELAY_MS - tecuClaimElapsedMs; - log("Init") << "Waiting " << remainingDelay << "ms after TECU address claim (J1939-81 250ms rule)..." << std::endl; - // Process CAN messages during the delay to prevent timeouts - auto delayStart = isobus::SystemTiming::get_timestamp_ms(); - while (isobus::SystemTiming::get_time_elapsed_ms(delayStart) < remainingDelay) - { - isobus::CANNetworkManager::CANNetwork.update(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } + isobus::CANNetworkManager::CANNetwork.update(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + // A primary that did not get 0xF0 lost it to another device: update_tecu() steps it down } else { tecuAddressClaimFailed = true; - if (tecuCF->get_address_valid()) - { - log("Warning") << "TECU claimed unexpected address " << static_cast(tecuCF->get_address()) << " instead of " << static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU) << "!" << std::endl; - } - else - { - log("Warning") << "TECU failed to claim address " << static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU) << "! Another TECU may be on the bus." << std::endl; - } - log("Warning") << "TECU functionality will be disabled." << std::endl; - tecuCF.reset(); // Release the failed control function + log("Warning") << "TECU failed to claim an address. TECU functionality will be disabled." << std::endl; + teardown_tecu(); + tecuRole = TecuRole::None; } } @@ -459,8 +458,10 @@ void Application::setup_task_controller_server() void Application::setup_tecu_interfaces() { - // Initialize speed and distance messages - if (tecuCF && tecuCF->get_address_valid()) + // Initialize speed and distance messages. A primary that did not get 0xF0 waits for update_tecu() + // to step it down instead of sending from another address. + if (tecuCF && tecuCF->get_address_valid() && + ((TecuRole::Primary != tecuRole) || (isobus::preferred_addresses::IndustryGroup2::TractorECU == tecuCF->get_address()))) { log("Init") << "Creating TECU Control Function Functionalities..." << std::endl; tecuFunctionalities = std::make_unique(tecuCF); @@ -474,17 +475,17 @@ void Application::setup_tecu_interfaces() true); log("Init") << "TECU announced as Class 1 Tractor ECU (PGN 64654)" << std::endl; + // The primary sends everything. A secondary sends nothing until it knows what the other + // Tractor ECUs provide (update_tecu()), so it never doubles one of their messages. + const TecuMessagePlan plan = plan_tecu_messages(TecuRole::Primary == tecuRole, Facilities{}, {}); + log("Init") << "Creating Speed Messages Interface on TECU..." << std::endl; - speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages - speedMessagesInterface->initialize(); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + create_speed_messages_interface(plan); log("Init") << "Speed Messages Interface created and initialized." << std::endl; log("Init") << "Creating NMEA2000 Message Interface on TECU..." << std::endl; - nmea2000MessageInterface = std::make_unique(tecuCF, settings->is_nmea_send_enabled(), false, false, false, false, false, false); + nmea2000MessageInterface = std::make_unique( + tecuCF, settings->is_nmea_send_enabled() && plan.sends(TecuMessage::CogSogRapidUpdate), false, false, false, false, false, false); nmea2000MessageInterface->initialize(); log("Init") << "NMEA2000 Message Interface created and initialized." << std::endl; @@ -492,7 +493,11 @@ void Application::setup_tecu_interfaces() tractorFacilities = std::make_unique(tecuCF, settings); tractorFacilities->set_speed_messages_interface(speedMessagesInterface.get()); tractorFacilities->set_nmea2000_message_interface(nmea2000MessageInterface.get()); + tractorFacilities->set_provided_facilities(facilities_for_plan(plan)); tractorFacilities->initialize(); + tecuPlan = plan; + tecuInterfacesReady = true; + tecuInterfacesReadyMs = isobus::SystemTiming::get_timestamp_ms(); // Register repetition-rate diagnostic on the TECU's PGN request protocol auto tecuPgnReq = tecuCF->get_pgn_request_protocol().lock(); @@ -502,9 +507,15 @@ void Application::setup_tecu_interfaces() static_cast(isobus::CANLibParameterGroupNumber::Any), &log_repetition_rate_request, nullptr); } - // Also catch broadcast PGN 0xCC00 messages that AgIsoStack warns about - isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( - 0xCC00 /* RequestForRepetitionRate */, &log_broadcast_repetition_rate, nullptr); + // Also catch broadcast PGN 0xCC00 messages that AgIsoStack warns about. + // Once only: this runs again whenever the TECU is rebuilt (update_tecu()). + static bool broadcastRepetitionRateListenerAdded = false; + if (!broadcastRepetitionRateListenerAdded) + { + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + 0xCC00 /* RequestForRepetitionRate */, &log_broadcast_repetition_rate, nullptr); + broadcastRepetitionRateListenerAdded = true; + } } else { @@ -519,6 +530,244 @@ void Application::setup_tecu_interfaces() } } +void Application::start_tecu_claim(TecuRole role, std::uint8_t functionInstance) +{ + // Arbitrary-address-capable as either role. That bit is the most significant one of the NAME, so a + // primary always loses an address claim contest for 0xF0 against a tractor's fixed-address Tractor + // ECU, instead of taking the address from it, and then moves elsewhere until update_tecu() steps it down. + isobus::NAME name = tecuNameTemplate; + name.set_arbitrary_address_capable(true); + name.set_function_instance(functionInstance); + const std::uint8_t preferredAddress = (TecuRole::Primary == role) ? isobus::preferred_addresses::IndustryGroup2::TractorECU : SECONDARY_TECU_PREFERRED_ADDRESS; + + log("TECU") << "Claiming an address as the " << ((TecuRole::Primary == role) ? "primary" : "secondary") + << " Tractor ECU (function instance " << static_cast(functionInstance) << ", preferred address " + << format_hex_address(preferredAddress) << ")" << std::endl; + tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(name, 0, preferredAddress); + tecuRole = role; + tecuFunctionInstance = functionInstance; + tecuClaimStartedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = 0; + tecuInterfacesReady = false; + tecuPlanSettled = (TecuRole::Primary == role); // The primary sends everything; nothing to settle + tecuPlan = TecuMessagePlan{}; +} + +void Application::teardown_tecu() +{ + // Interfaces first: their destructors unregister from the control function's PGN request protocol, + // which deactivate_control_function() releases + tractorFacilities.reset(); + speedMessagesInterface.reset(); + nmea2000MessageInterface.reset(); + tecuFunctionalities.reset(); + if (tecuCF) + { + isobus::CANNetworkManager::CANNetwork.deactivate_control_function(tecuCF); + tecuCF.reset(); + } + tecuInterfacesReady = false; + tecuPlanSettled = false; + tecuPlan = TecuMessagePlan{}; + tractorFacilitiesSentOnPowerUp = false; +} + +void Application::create_speed_messages_interface(const TecuMessagePlan &plan) +{ + // The stack fixes which speed messages are sent when the interface is constructed, so a changed plan + // needs a new one. Carry the current values over so implements never see a zero speed in between. + auto previous = std::move(speedMessagesInterface); + speedMessagesInterface = std::make_unique(tecuCF, + plan.sends(TecuMessage::GroundBasedSpeed), + plan.sends(TecuMessage::WheelBasedSpeed), + plan.sends(TecuMessage::MachineSelectedSpeed), + false); + speedMessagesInterface->initialize(); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + + if (previous) + { + auto &next = *speedMessagesInterface; + next.groundBasedSpeedTransmitData.set_machine_speed(previous->groundBasedSpeedTransmitData.get_machine_speed()); + next.groundBasedSpeedTransmitData.set_machine_distance(previous->groundBasedSpeedTransmitData.get_machine_distance()); + next.groundBasedSpeedTransmitData.set_machine_direction_of_travel(previous->groundBasedSpeedTransmitData.get_machine_direction_of_travel()); + next.wheelBasedSpeedTransmitData.set_machine_speed(previous->wheelBasedSpeedTransmitData.get_machine_speed()); + next.wheelBasedSpeedTransmitData.set_machine_distance(previous->wheelBasedSpeedTransmitData.get_machine_distance()); + next.wheelBasedSpeedTransmitData.set_machine_direction_of_travel(previous->wheelBasedSpeedTransmitData.get_machine_direction_of_travel()); + next.machineSelectedSpeedTransmitData.set_machine_speed(previous->machineSelectedSpeedTransmitData.get_machine_speed()); + next.machineSelectedSpeedTransmitData.set_machine_distance(previous->machineSelectedSpeedTransmitData.get_machine_distance()); + next.machineSelectedSpeedTransmitData.set_machine_direction_of_travel(previous->machineSelectedSpeedTransmitData.get_machine_direction_of_travel()); + } + if (tractorFacilities) + { + tractorFacilities->set_speed_messages_interface(speedMessagesInterface.get()); + } +} + +void Application::apply_tecu_plan(const TecuMessagePlan &plan) +{ + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + if (plan.send[i] != tecuPlan.send[i]) + { + const auto message = static_cast(i); + log("TECU") << (plan.send[i] ? "Sending " : "Not sending ") << tecu_message_name(message) + << (plan.send[i] ? ": no other Tractor ECU provides it" : ": another Tractor ECU provides it") << std::endl; + } + } + + const bool speedMessagesChanged = (plan.sends(TecuMessage::GroundBasedSpeed) != tecuPlan.sends(TecuMessage::GroundBasedSpeed)) || + (plan.sends(TecuMessage::WheelBasedSpeed) != tecuPlan.sends(TecuMessage::WheelBasedSpeed)) || + (plan.sends(TecuMessage::MachineSelectedSpeed) != tecuPlan.sends(TecuMessage::MachineSelectedSpeed)); + if (speedMessagesChanged) + { + create_speed_messages_interface(plan); + } + if (nmea2000MessageInterface) + { + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(settings->is_nmea_send_enabled() && plan.sends(TecuMessage::CogSogRapidUpdate)); + } + tecuPlan = plan; + + if (tractorFacilities) + { + tractorFacilities->set_provided_facilities(facilities_for_plan(plan)); + if (tractorFacilitiesSentOnPowerUp) + { + // Implements that cached our earlier response learn what changed + tractorFacilities->send_facilities_response(); + } + } +} + +void Application::update_tecu() +{ + if (TecuRole::None == tecuRole) + { + return; + } + + auto step_down = [this](const std::string &reason) { + // ISO 11783-9 4.4.2.1: the tractor's own Tractor ECU is the primary; we become a secondary, + // with a function instance above every Tractor ECU on the bus + log("TECU") << reason << ": stepping down to a secondary Tractor ECU that only adds what it lacks" << std::endl; + send_hardware_message("Tractor ECU found: AOG adds only missing data", 10, HW_MSG_INFO); + const auto all = otherTecus.refresh(ALL_FUNCTION_INSTANCES); + const int instance = all.empty() ? 1 : std::min(all.back().functionInstance + 1, ALL_FUNCTION_INSTANCES); + teardown_tecu(); + start_tecu_claim(TecuRole::Secondary, static_cast(instance)); + }; + + // A primary claim that did not end on 0xF0 lost it to another device: do not start sending from there + if ((TecuRole::Primary == tecuRole) && tecuCF && tecuCF->get_address_valid() && + (isobus::preferred_addresses::IndustryGroup2::TractorECU != tecuCF->get_address())) + { + step_down("Address 0xF0 is taken"); + return; + } + + // A claim started at runtime: finish it without blocking the main loop + if (!tecuInterfacesReady) + { + if (tecuCF && tecuCF->get_address_valid()) + { + if (0 == tecuClaimedMs) + { + tecuClaimedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = (0 == tecuClaimedMs) ? 1 : tecuClaimedMs; + log("TECU") << "Claimed address " << format_hex_address(tecuCF->get_address()) << std::endl; + } + if (isobus::SystemTiming::time_expired_ms(tecuClaimedMs, 250)) // J1939-81 250 ms rule + { + setup_tecu_interfaces(); + } + } + else if (isobus::SystemTiming::time_expired_ms(tecuClaimStartedMs, TECU_CLAIM_TIMEOUT_MS)) + { + log("Warning") << "TECU failed to claim an address. TECU functionality will be disabled." << std::endl; + tecuAddressClaimFailed = true; + teardown_tecu(); + tecuRole = TecuRole::None; + } + return; + } + + if (!isobus::SystemTiming::time_expired_ms(lastTecuCheckMs, TECU_CHECK_INTERVAL_MS)) + { + return; + } + lastTecuCheckMs = isobus::SystemTiming::get_timestamp_ms(); + + // Only Tractor ECUs with a lower function instance count (ISO 11783-9 4.4.2.1 c). For the primary + // (instance 0) that is another instance 0: the tractor's own; a secondary appearing is no reason to step down. + const std::uint8_t deferTo = (TecuRole::Primary == tecuRole) ? 0 : static_cast(tecuFunctionInstance - 1); + const auto others = otherTecus.refresh(deferTo); + std::set addresses; + for (const auto &other : others) + { + addresses.insert(other.address); + if (0 == knownOtherTecuAddresses.count(other.address)) + { + log("TECU") << "Another Tractor ECU appeared at " << format_hex_address(other.address) << " (function instance " + << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + } + } + for (const auto address : knownOtherTecuAddresses) + { + if (0 == addresses.count(address)) + { + log("TECU") << "The Tractor ECU at " << format_hex_address(address) << " left the bus" << std::endl; + } + } + knownOtherTecuAddresses = addresses; + + if (TecuRole::Primary == tecuRole) + { + if (!others.empty()) + { + step_down("A Tractor ECU is on the bus"); + } + else if (!tecuCF->get_address_valid()) + { + step_down("Lost address 0xF0"); + } + return; + } + + // Secondary: ask each other TECU what it provides (ISO 11783-9 4.4.2.1 d) + for (const auto &other : others) + { + if (!otherTecus.facilities_requested(other.address) && + isobus::ParameterGroupNumberRequestProtocol::request_parameter_group_number(TractorFacilities::PGN_TRACTOR_FACILITIES, tecuCF, other.controlFunction)) + { + otherTecus.mark_facilities_requested(other.address); + log("TECU") << "Requested tractor facilities (PGN 65033) from " << format_hex_address(other.address) << std::endl; + } + } + + if (!tecuPlanSettled) + { + const std::uint32_t elapsed = isobus::SystemTiming::get_time_elapsed_ms(tecuInterfacesReadyMs); + const bool answered = otherTecus.all_facilities_received() && (elapsed >= TECU_PLAN_MIN_SETTLE_MS); + if (!answered && (elapsed < TECU_PLAN_SETTLE_MS)) + { + return; + } + tecuPlanSettled = true; + log("TECU") << (answered ? "The other Tractor ECUs reported their facilities" : "Not every Tractor ECU answered PGN 65033; going by what they send") + << std::endl; + } + + const TecuMessagePlan plan = plan_tecu_messages(false, otherTecus.advertised_facilities(), otherTecus.sent_by_others()); + if (plan != tecuPlan) + { + apply_tecu_plan(plan); + } +} + void Application::setup_udp_connections() { static std::uint8_t xteSid = 0; @@ -704,6 +953,7 @@ bool Application::update() udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); + update_tecu(); tcServer->request_measurement_commands(); warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); @@ -725,7 +975,8 @@ bool Application::update() // The send fails until the TECU has claimed an address (or if the transmit queue rejects it), // so only stop once it went out and retry once a second until then. static std::uint32_t lastFacilitiesAttemptMs = 0; - if (!tractorFacilitiesSentOnPowerUp && tractorFacilities && + // A secondary TECU waits until it knows what the other Tractor ECUs provide. + if (!tractorFacilitiesSentOnPowerUp && tractorFacilities && tecuPlanSettled && isobus::SystemTiming::time_expired_ms(lastFacilitiesAttemptMs, 1000)) { lastFacilitiesAttemptMs = isobus::SystemTiming::get_timestamp_ms(); @@ -805,7 +1056,7 @@ bool Application::update() // This is deferred from setup_control_functions() until UDP is ready. if (tecuAddressClaimFailed) { - send_hardware_message("TECU failed to claim address " + std::to_string(static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU)), 10, HW_MSG_ALERT); + send_hardware_message("TECU failed to claim an address", 10, HW_MSG_ALERT); tecuAddressClaimFailed = false; } @@ -855,7 +1106,8 @@ bool Application::update() } // Send J1939 PGN 65256 every 100ms (0.1 seconds) - if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF && tecuCF->get_address_valid()) + if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF && tecuCF->get_address_valid() && + tecuPlan.sends(TecuMessage::VehicleDirectionSpeed)) { std::uint16_t speed_j1939 = (static_cast(std::abs(lastSpeedValue)) * 576u + 312u) / 625u; // mm/s -> (km/h)*256 std::array j1939_speed_data = { @@ -1281,7 +1533,7 @@ void Application::setup_vt_client() saved = settings->set_nmea_send_enabled(enabled); if (saved) { - nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(enabled); + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(enabled && tecuPlan.sends(TecuMessage::CogSogRapidUpdate)); } } else diff --git a/src/tecu_coexistence.cpp b/src/tecu_coexistence.cpp new file mode 100644 index 0000000..11811e9 --- /dev/null +++ b/src/tecu_coexistence.cpp @@ -0,0 +1,292 @@ +/** + * @brief Sharing the implement bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1). + */ + +#include "tecu_coexistence.hpp" + +#include "isobus/isobus/can_NAME.hpp" +#include "isobus/isobus/can_network_manager.hpp" +#include "isobus/utility/system_timing.hpp" + +#include + +namespace +{ + constexpr std::size_t index_of(TecuMessage message) + { + return static_cast(message); + } +} + +std::uint32_t tecu_message_pgn(TecuMessage message) +{ + switch (message) + { + case TecuMessage::GroundBasedSpeed: + return 65097; + case TecuMessage::WheelBasedSpeed: + return 65096; + case TecuMessage::MachineSelectedSpeed: + return 61474; + case TecuMessage::VehicleDirectionSpeed: + return 65256; + case TecuMessage::CogSogRapidUpdate: + return 129026; + case TecuMessage::TimeDate: + return 65254; + default: + return 0; + } +} + +std::string tecu_message_name(TecuMessage message) +{ + std::string name; + switch (message) + { + case TecuMessage::GroundBasedSpeed: + name = "ground-based speed"; + break; + case TecuMessage::WheelBasedSpeed: + name = "wheel-based speed"; + break; + case TecuMessage::MachineSelectedSpeed: + name = "machine selected speed"; + break; + case TecuMessage::VehicleDirectionSpeed: + name = "vehicle direction/speed"; + break; + case TecuMessage::CogSogRapidUpdate: + name = "NMEA 2000 COG/SOG"; + break; + case TecuMessage::TimeDate: + name = "time/date"; + break; + default: + name = "unknown"; + break; + } + return name + " (PGN " + std::to_string(tecu_message_pgn(message)) + ")"; +} + +TecuMessagePlan plan_tecu_messages(bool isPrimary, + const Facilities &advertisedByOthers, + const std::array &sentByOthers) +{ + TecuMessagePlan plan; + if (isPrimary) + { + plan.send.fill(true); + return plan; + } + + std::array advertised{}; + advertised[index_of(TecuMessage::GroundBasedSpeed)] = advertisedByOthers.groundBasedSpeed; + advertised[index_of(TecuMessage::WheelBasedSpeed)] = advertisedByOthers.wheelBasedSpeed; + advertised[index_of(TecuMessage::MachineSelectedSpeed)] = advertisedByOthers.selectedSpeed; + advertised[index_of(TecuMessage::TimeDate)] = advertisedByOthers.timeDate; + // Vehicle direction/speed and COG/SOG have no facility bit: only what is seen on the bus counts + + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + plan.send[i] = !(advertised[i] || sentByOthers[i]); + } + return plan; +} + +Facilities facilities_for_plan(const TecuMessagePlan &plan) +{ + Facilities f; + f.tecuClass = 1; + + const bool ground = plan.sends(TecuMessage::GroundBasedSpeed); + f.groundBasedSpeed = ground; + f.groundBasedDistance = ground; + f.groundBasedDirection = ground; + + const bool wheel = plan.sends(TecuMessage::WheelBasedSpeed); + f.wheelBasedSpeed = wheel; + f.wheelBasedDistance = wheel; + f.wheelBasedDirection = wheel; + + f.selectedSpeed = plan.sends(TecuMessage::MachineSelectedSpeed); + return f; +} + +Facilities merge_facilities(const Facilities &a, const Facilities &b) +{ + // The encoded payload is the bitwise form of every facility, so OR it byte by byte + const auto pa = encode_facilities(a); + const auto pb = encode_facilities(b); + std::array merged{}; + for (std::size_t i = 0; i < merged.size(); i++) + { + merged[i] = static_cast(pa[i] | pb[i]); + } + Facilities result = decode_facilities(merged); + // The class field is not a bit set: keep the higher class ("not available" is 0 here) + result.tecuClass = std::max(a.tecuClass, b.tecuClass); + return result; +} + +std::uint32_t pgn_from_can_identifier(std::uint32_t identifier) +{ + const std::uint32_t pduFormat = (identifier >> 16) & 0xFF; + const std::uint32_t pduSpecific = (identifier >> 8) & 0xFF; + const std::uint32_t dataPages = (identifier >> 24) & 0x03; // Extended data page and data page + // PDU1 (PF < 240): the PS byte is a destination address, not part of the PGN + return (dataPages << 16) | (pduFormat << 8) | ((pduFormat >= 0xF0) ? pduSpecific : 0); +} + +void OtherTecuMonitor::initialize() +{ + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + TractorFacilities::PGN_TRACTOR_FACILITIES, &OtherTecuMonitor::on_facilities_response, this); +} + +std::vector OtherTecuMonitor::refresh(std::uint8_t maxFunctionInstance) +{ + std::vector others; + for (const auto &cf : isobus::CANNetworkManager::CANNetwork.get_control_functions(false)) + { + if ((nullptr == cf) || !cf->get_address_valid() || + (isobus::ControlFunction::Type::Internal == cf->get_type()) || + (static_cast(isobus::NAME::Function::TractorECU) != cf->get_NAME().get_function_code()) || + (cf->get_NAME().get_function_instance() > maxFunctionInstance)) + { + continue; + } + OtherTecu other; + other.controlFunction = cf; + other.address = cf->get_address(); + other.functionInstance = cf->get_NAME().get_function_instance(); + other.manufacturerCode = cf->get_NAME().get_manufacturer_code(); + others.push_back(other); + } + std::sort(others.begin(), others.end(), [](const OtherTecu &a, const OtherTecu &b) { + return a.functionInstance < b.functionInstance; + }); + + std::set addresses; + std::map names; + for (const auto &other : others) + { + addresses.insert(other.address); + names[other.address] = other.controlFunction->get_NAME().get_full_name(); + } + + std::lock_guard lock(mutex); + for (std::size_t address = 0; address < isOtherTecuAddress.size(); address++) + { + isOtherTecuAddress[address] = (addresses.count(static_cast(address)) > 0); + } + for (auto it = facilitiesByAddress.begin(); it != facilitiesByAddress.end();) + { + // A TECU that left, or a different one now at that address, has to answer again + const auto name = names.find(it->first); + const auto previous = nameByAddress.find(it->first); + const bool same = (names.end() != name) && (nameByAddress.end() != previous) && (name->second == previous->second); + it = same ? std::next(it) : facilitiesByAddress.erase(it); + } + for (auto it = requestedAddresses.begin(); it != requestedAddresses.end();) + { + const auto name = names.find(*it); + const auto previous = nameByAddress.find(*it); + const bool same = (names.end() != name) && (nameByAddress.end() != previous) && (name->second == previous->second); + it = same ? std::next(it) : requestedAddresses.erase(it); + } + otherAddresses = addresses; + nameByAddress = names; + return others; +} + +void OtherTecuMonitor::on_frame(const isobus::CANMessageFrame &frame) +{ + if (!frame.isExtendedFrame || !isOtherTecuAddress[frame.identifier & 0xFF]) + { + return; + } + const std::uint32_t pgn = pgn_from_can_identifier(frame.identifier); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + if (tecu_message_pgn(static_cast(i)) == pgn) + { + const std::uint32_t now = isobus::SystemTiming::get_timestamp_ms(); + lastSentByOthersMs[i] = (0 == now) ? 1 : now; // 0 means "never" + } + } +} + +void OtherTecuMonitor::on_facilities_response(const isobus::CANMessage &message, void *parent) +{ + auto *self = static_cast(parent); + const auto source = message.get_source_control_function(); + const auto &data = message.get_data(); + if ((nullptr == self) || (nullptr == source) || (data.size() < 8)) + { + return; + } + const std::uint8_t address = source->get_address(); + if (!self->isOtherTecuAddress[address]) + { + return; // Our own response, or one from something that is not a TECU + } + + std::array payload{}; + std::copy(data.begin(), data.begin() + 8, payload.begin()); + if (payload[7] & 0x01) + { + // Reserved bit indicator set (ISO 11783-7 A.27.6): the newer facility bits are 1-filled, not real + payload[1] &= 0xFC; + payload[3] &= 0xF0; + payload[4] &= 0xE0; + payload[6] = 0; + payload[7] = 0; + } + + std::lock_guard lock(self->mutex); + self->facilitiesByAddress[address] = decode_facilities(payload); +} + +Facilities OtherTecuMonitor::advertised_facilities() const +{ + std::lock_guard lock(mutex); + Facilities combined; + combined.tecuClass = 0; + for (const auto &entry : facilitiesByAddress) + { + combined = merge_facilities(combined, entry.second); + } + return combined; +} + +bool OtherTecuMonitor::all_facilities_received() const +{ + std::lock_guard lock(mutex); + return std::all_of(otherAddresses.begin(), otherAddresses.end(), [this](std::uint8_t address) { + return facilitiesByAddress.count(address) > 0; + }); +} + +std::array OtherTecuMonitor::sent_by_others() const +{ + std::array sent{}; + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + const std::uint32_t last = lastSentByOthersMs[i]; + sent[i] = (0 != last) && !isobus::SystemTiming::time_expired_ms(last, SENT_BY_OTHERS_WINDOW_MS); + } + return sent; +} + +bool OtherTecuMonitor::facilities_requested(std::uint8_t address) const +{ + std::lock_guard lock(mutex); + return requestedAddresses.count(address) > 0; +} + +void OtherTecuMonitor::mark_facilities_requested(std::uint8_t address) +{ + std::lock_guard lock(mutex); + requestedAddresses.insert(address); +} diff --git a/src/tractor_facilities.cpp b/src/tractor_facilities.cpp index f071620..ed80093 100644 --- a/src/tractor_facilities.cpp +++ b/src/tractor_facilities.cpp @@ -254,6 +254,30 @@ TractorFacilities::TractorFacilities( { } +TractorFacilities::~TractorFacilities() +{ + if (!initialized) + { + return; + } + // The request protocol calls on_pgn_request() under its own lock, so after this returns it no longer + // runs. The stack may still call on_required_facilities() once after removal (it iterates a copy of + // the global callbacks), which is harmless: that callback only logs and never touches this object. + if (auto pgnRequestProtocol = tecuCF ? tecuCF->get_pgn_request_protocol().lock() : nullptr) + { + pgnRequestProtocol->remove_pgn_request_callback(PGN_TRACTOR_FACILITIES, &TractorFacilities::on_pgn_request, this); + } + isobus::CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback( + PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); +} + +void TractorFacilities::set_provided_facilities(const Facilities &facilities) +{ + std::lock_guard lock(providedFacilitiesMutex); + providedFacilities = facilities; + hasProvidedFacilities = true; +} + void TractorFacilities::set_speed_messages_interface(isobus::SpeedMessagesInterface *iface) { speedMessagesInterface = iface; @@ -271,6 +295,16 @@ void TractorFacilities::set_time_date_active(bool active) std::array TractorFacilities::build_payload() const { + { + std::lock_guard lock(providedFacilitiesMutex); + if (hasProvidedFacilities) + { + Facilities provided = providedFacilities; + provided.timeDate = timeDateActive; + return encode_facilities(provided); + } + } + Facilities f; f.tecuClass = 1; // Class 1 – the only class we claim. @@ -333,6 +367,7 @@ bool TractorFacilities::initialize() PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); std::cout << "[" << get_timestamp() << "] [TractorFacilities] Registered PGN 65032 diagnostic listener." << std::endl; + initialized = true; return true; } diff --git a/tools/test_tecu_coexistence.cpp b/tools/test_tecu_coexistence.cpp new file mode 100644 index 0000000..850cb6c --- /dev/null +++ b/tools/test_tecu_coexistence.cpp @@ -0,0 +1,157 @@ +/** + * @brief Unit tests for sharing the bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1): + * which messages a secondary TECU sends, what it advertises, and PGN decoding. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "tecu_coexistence.hpp" + +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +static std::array nothing_sent() +{ + return {}; +} + +static void test_primary_sends_everything() +{ + std::printf("test_primary_sends_everything\n"); + Facilities others; + others.groundBasedSpeed = true; + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::WheelBasedSpeed)] = true; + + const auto plan = plan_tecu_messages(true, others, sent); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + check(plan.send[i], "primary sends every message, whatever others do"); + } +} + +static void test_secondary_alone_sends_everything() +{ + std::printf("test_secondary_alone_sends_everything\n"); + const auto plan = plan_tecu_messages(false, Facilities{}, nothing_sent()); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + check(plan.send[i], "secondary with nothing provided by others sends every message"); + } +} + +// The ISO 11783-9 example: the tractor has wheel sensors but no ground-based speed; the display adds it from GNSS +static void test_secondary_adds_ground_speed_only() +{ + std::printf("test_secondary_adds_ground_speed_only\n"); + Facilities tractor; + tractor.wheelBasedSpeed = true; + tractor.wheelBasedDistance = true; + tractor.wheelBasedDirection = true; + tractor.selectedSpeed = true; + tractor.timeDate = true; + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::VehicleDirectionSpeed)] = true; + + const auto plan = plan_tecu_messages(false, tractor, sent); + check(plan.sends(TecuMessage::GroundBasedSpeed), "sends ground-based speed (tractor lacks it)"); + check(!plan.sends(TecuMessage::WheelBasedSpeed), "leaves wheel-based speed to the tractor"); + check(!plan.sends(TecuMessage::MachineSelectedSpeed), "leaves machine selected speed to the tractor"); + check(!plan.sends(TecuMessage::TimeDate), "leaves time/date to the tractor"); + check(!plan.sends(TecuMessage::VehicleDirectionSpeed), "does not double a message the tractor sends without a facility bit"); + check(plan.sends(TecuMessage::CogSogRapidUpdate), "sends COG/SOG, which the tractor neither advertises nor sends"); + + const Facilities ours = facilities_for_plan(plan); + check(ours.tecuClass == 1, "advertises class 1"); + check(ours.groundBasedSpeed && ours.groundBasedDistance && ours.groundBasedDirection, "advertises ground-based speed, distance, direction"); + check(!ours.wheelBasedSpeed && !ours.wheelBasedDistance && !ours.wheelBasedDirection, "does not advertise wheel-based facilities"); + check(!ours.selectedSpeed, "does not advertise selected speed"); + check(!ours.timeDate, "time/date bit is left to the time/date code"); + + // ISO 11783-9 4.4.2.1 d: no facility bit set by both + const auto tractorPayload = encode_facilities(tractor); + const auto ourPayload = encode_facilities(ours); + for (std::size_t i = 1; i < tractorPayload.size(); i++) + { + check(0 == (tractorPayload[i] & ourPayload[i]), "no facility bit advertised by both TECUs"); + } + check(0 == (tractorPayload[0] & ourPayload[0] & 0x3F), "no byte 1 facility bit advertised by both TECUs"); +} + +static void test_seen_on_bus_counts_without_facility_bit() +{ + std::printf("test_seen_on_bus_counts_without_facility_bit\n"); + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::GroundBasedSpeed)] = true; // sent but not advertised + const auto plan = plan_tecu_messages(false, Facilities{}, sent); + check(!plan.sends(TecuMessage::GroundBasedSpeed), "a message seen from another TECU is not sent, advertised or not"); + check(plan.sends(TecuMessage::WheelBasedSpeed), "others are still sent"); +} + +static void test_primary_advertises_selected_speed() +{ + std::printf("test_primary_advertises_selected_speed\n"); + const Facilities ours = facilities_for_plan(plan_tecu_messages(true, Facilities{}, nothing_sent())); + const auto payload = encode_facilities(ours); + check(payload[0] == 0x06, "byte 1: class 1, ground- and wheel-based speed"); + check(payload[2] == 0x78, "byte 3: ground- and wheel-based distance and direction"); + check(payload[4] == 0x04, "byte 5: selected speed (ISO 11783-7 B.24.3 byte 5 bit 3)"); +} + +static void test_merge_facilities() +{ + std::printf("test_merge_facilities\n"); + Facilities a; + a.tecuClass = 1; + a.wheelBasedSpeed = true; + Facilities b; + b.tecuClass = 2; + b.timeDate = true; + b.frontHitchPosition = true; + const Facilities merged = merge_facilities(a, b); + check(merged.wheelBasedSpeed && merged.timeDate && merged.frontHitchPosition, "merge keeps every facility of both"); + check(!merged.groundBasedSpeed, "merge does not invent facilities"); + check(merged.tecuClass == 2, "merge keeps the higher class"); +} + +static void test_pgn_from_can_identifier() +{ + std::printf("test_pgn_from_can_identifier\n"); + check(pgn_from_can_identifier(0x0CFE49F0) == 65097, "ground-based speed from 0xF0"); + check(pgn_from_can_identifier(0x0CFE4826) == 65096, "wheel-based speed from 0x26"); + check(pgn_from_can_identifier(0x0CF022F0) == 61474, "machine selected speed (PDU2, PS is part of the PGN)"); + check(pgn_from_can_identifier(0x18FEE6F0) == 65254, "time/date"); + check(pgn_from_can_identifier(0x09F80281) == 129026, "NMEA 2000 COG/SOG (data page 1)"); + check(pgn_from_can_identifier(0x18EA80F0) == 0xEA00, "PDU1 request: destination address is not part of the PGN"); + check(tecu_message_pgn(TecuMessage::CogSogRapidUpdate) == 129026, "COG/SOG PGN"); +} + +int main() +{ + test_primary_sends_everything(); + test_secondary_alone_sends_everything(); + test_secondary_adds_ground_speed_only(); + test_seen_on_bus_counts_without_facility_bit(); + test_primary_advertises_selected_speed(); + test_merge_facilities(); + test_pgn_from_can_identifier(); + + if (failures > 0) + { + std::fprintf(stderr, "\n%d check(s) FAILED\n", failures); + return 1; + } + std::printf("\nAll checks passed\n"); + return 0; +} From 0b25d97e64bd3d9e11f678412f3e8cebb13f7163 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 16:27:48 +0100 Subject: [PATCH 14/22] Fill the tractor's gaps when tecuEnabled is off tecuEnabled off is what operators set when the tractor has its own Tractor ECU, and so far it meant no TECU at all. Now it starts our TECU as a secondary, never at 0xF0: it listens to the tractor's Tractor ECU and sends only what that one lacks, or everything when there is none. tecuEnabled on still makes AOG the primary, stepping down if a tractor TECU turns up after all. The VT label shows who leads: "TECU: AOG" or "TECU: tractor" (pool length 23). Co-Authored-By: Claude Opus 5.5 --- docs/PROTOCOL.md | 10 +++++----- readme.md | 4 ++-- src/app.cpp | 37 +++++++++++++++++++------------------ 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 054e29f..37bc229 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -217,8 +217,8 @@ The TC reads `settings.json` from a per-user config directory: | Key | Type | Default | Description | |---|---|---|---| | `subnet` | `int[3]` | `[192, 168, 5]` | First three octets of the LAN AgIO/AgValonia lives on. Used for NIC selection and broadcast destination. | -| `tecuEnabled` | `bool` | `true` | If `true`, the TC also impersonates a Tractor ECU on the CAN bus (claims address 240, broadcasts Speed Messages/NMEA2000, announces Class 1 BasicTractorECUServer). Set `false` when the tractor already has a TECU. | -| `nmeaSendEnabled` | `bool` | `true` | Enable cyclic NMEA2000 COG/SOG transmission. Requires `tecuEnabled`; the VT disables this control when no TECU interface exists. | +| `tecuEnabled` | `bool` | `true` | `true`: the TC is the tractor's Tractor ECU, the primary at address 240 (Speed Messages, NMEA2000, Class 1 BasicTractorECUServer), stepping down if the tractor has one after all. `false`: for tractors with their own TECU; the TC's TECU is a secondary that only adds what the tractor's lacks, and everything when there is none. See §5.7. | +| `nmeaSendEnabled` | `bool` | `true` | Enable cyclic NMEA2000 COG/SOG transmission (not while another Tractor ECU sends it, §5.7). The VT disables this control when no TECU interface exists. | | `aogHeartbeatEnabled` | `bool` | `true` | Send `0xF0` heartbeat to AgIO/AgValonia every 100 ms even with no implement. Disable for AOG < v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Register the Virtual Terminal client and display the TC UI when a VT is present. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`=DIS, `1`=FDIS.1, `2`=First Edition, `3`=Second Edition Draft, `4`=Second Published Edition). | @@ -257,7 +257,7 @@ Two control functions, both claiming addresses via standard J1939-81 address cla | CF | NAME function | Address | Notes | |---|---|---|---| | Task Controller | `TaskController` (function code 61) | Preferred `247` (ISO 11783-10 MappingComputer). Walks if claimed. | Always present. | -| Tractor ECU | `TractorECU` (function code 134) | `240` as the primary; an arbitrary address (preferred `128`) as a secondary, see §5.7. | Only present if `tecuEnabled: true`. | +| Tractor ECU | `TractorECU` (function code 134) | `240` as the primary; an arbitrary address (preferred `128`) as a secondary, see §5.7. | Primary with `tecuEnabled: true`, secondary with `false`. | Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufacturer Code `1407`, Identity `20`. Override these in `app.cpp` if you fork. @@ -339,9 +339,9 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 ISO 11783-9:2012 §4.4.2.1 allows several Tractor ECUs on the implement bus. The one with function instance 0 is the primary; one with a higher instance "shall not provide any messages which are provided by a Tractor ECU with a lower function instance", requests PGN 65033 from those, and sets only the facility bits they lack (the standard's example is a display adding ground-based speed from GNSS to a tractor without radar). -- **At startup** the TC's own address claim makes every control function on the bus announce itself. If no other Tractor ECU (function code 134) is there, our TECU claims `240` as the primary (function instance 0) and sends everything in §5.2. If one is there, it claims an arbitrary address as a secondary, with the next function instance. +- **`tecuEnabled` picks the role.** `true`: AOG is the tractor's TECU. At startup the TC's own address claim makes every control function on the bus announce itself; if no other Tractor ECU (function code 134) is there, our TECU claims `240` as the primary (function instance 0) and sends everything in §5.2, otherwise it starts as a secondary. `false` ("the tractor has its own TECU"): always a secondary at an arbitrary address, with a function instance above every Tractor ECU on the bus. With no other Tractor ECU there, every message is a gap and it sends them all; once the tractor's appears, it cuts back to what that one lacks. - **Never taking `240` from a tractor:** our TECU NAME is arbitrary-address-capable in both roles. That is the most significant NAME bit, so in an address claim contest for `240` our primary loses to a fixed-address Tractor ECU and moves elsewhere, instead of knocking the tractor's ECU off the bus. -- **Stepping down at runtime:** once a second the TC checks for other Tractor ECUs. If another one with function instance 0 appears while ours is the primary, or ours lost `240`, our TECU is rebuilt as a secondary above every Tractor ECU's function instance. Another secondary appearing is no reason to step down. The operator sees a hardware message ("Tractor ECU found: AOG adds only missing data"). +- **Stepping down at runtime** (`tecuEnabled: true`): once a second the TC checks for other Tractor ECUs. If another one with function instance 0 appears while ours is the primary, or ours lost `240`, our TECU is rebuilt as a secondary above every Tractor ECU's function instance. Another secondary appearing is no reason to step down. The operator sees a hardware message ("Tractor ECU found: AOG adds only missing data"). - **What a secondary sends:** nothing until the other Tractor ECUs have answered PGN 65033 (and at least 1 s has passed) or 2.5 s have passed. Then each of ground-based speed, wheel-based speed, machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG is sent only if no Tractor ECU with a lower function instance advertises it in PGN 65033 *and* none has sent it in the last 3 s. Only lower instances count, so two secondaries never wait on each other. Its PGN 65033 advertises exactly what it sends, and is re-sent when that changes. - A secondary does not step back up to primary if the tractor's Tractor ECU leaves; it then sends everything, still as a secondary. diff --git a/readme.md b/readme.md index 8c67159..37963bf 100644 --- a/readme.md +++ b/readme.md @@ -60,8 +60,8 @@ AOG-TaskController reads its configuration from a `settings.json` file located i | Key | Type | Default | Description | |---|---|---|---| | `subnet` | `int[3]` | `[192, 168, 5]` | LAN prefix used to select the AOG-facing NIC and broadcast address. | -| `tecuEnabled` | `bool` | `true` | Enables the internal Tractor ECU. Disable it when the tractor already has a TECU. A change made from the VT takes effect after restart. | -| `nmeaSendEnabled` | `bool` | `true` | Enables cyclic NMEA2000 COG/SOG transmission. Requires `tecuEnabled`. | +| `tecuEnabled` | `bool` | `true` | `true`: AOG is the tractor's Tractor ECU (address 240), and steps back if the tractor turns out to have one. `false`: use when the tractor already has a TECU; AOG then only adds what it lacks, such as ground-based speed from GNSS (ISO 11783-9 §4.4.2.1). A change made from the VT takes effect after restart. | +| `nmeaSendEnabled` | `bool` | `true` | Enables cyclic NMEA2000 COG/SOG transmission (not while the tractor's TECU sends it). | | `aogHeartbeatEnabled` | `bool` | `true` | Sends the 100 ms zero-section heartbeat when no implement is connected. Disable for AgOpenGPS versions before v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Enables the ISOBUS Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code, from `0` through `4`; `3` is Second Edition Draft. | diff --git a/src/app.cpp b/src/app.cpp index 5f2f334..f2d334f 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -343,27 +343,35 @@ bool Application::setup_control_functions() // Create TECU control function. By now the TC's own address claim made every CF on the bus // announce itself, so a tractor's Tractor ECU is already known (ISO 11783-9 4.4.2.1). - if (tcCF && settings->is_tecu_enabled()) - { // Only create TECU if TC was created and ECU is enabled + // tecuEnabled on: AOG is the tractor's TECU (primary at 0xF0), stepping down if one turns up anyway. + // tecuEnabled off ("the tractor has its own TECU"): a secondary that fills the gaps, never at 0xF0; + // with no other Tractor ECU on the bus, every message is a gap. + if (tcCF) + { // Only create TECU if TC was created otherTecus.initialize(); isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener([this](const isobus::CANMessageFrame &frame) { otherTecus.on_frame(frame); // CAN thread: notes what the other TECUs send }); const auto others = otherTecus.refresh(ALL_FUNCTION_INSTANCES); - if (others.empty()) + for (const auto &other : others) + { + knownOtherTecuAddresses.insert(other.address); + log("Init") << "Tractor ECU already on the bus at " << format_hex_address(other.address) << " (function instance " + << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + } + if (settings->is_tecu_enabled() && others.empty()) { start_tecu_claim(TecuRole::Primary, 0); } else { - for (const auto &other : others) + if (!settings->is_tecu_enabled()) { - knownOtherTecuAddresses.insert(other.address); - log("Init") << "Tractor ECU already on the bus at " << format_hex_address(other.address) << " (function instance " - << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + log("Init") << "tecuEnabled is off: the TECU only fills in what the tractor's Tractor ECU does not provide" << std::endl; } - start_tecu_claim(TecuRole::Secondary, static_cast(std::min(others.back().functionInstance + 1, ALL_FUNCTION_INSTANCES))); + const int instance = others.empty() ? 1 : std::min(others.back().functionInstance + 1, ALL_FUNCTION_INSTANCES); + start_tecu_claim(TecuRole::Secondary, static_cast(instance)); } // Wait for the claim here at startup, as before, so the TECU interfaces exist once setup is done. @@ -519,14 +527,7 @@ void Application::setup_tecu_interfaces() } else { - if (!settings->is_tecu_enabled()) - { - log("Info") << "Tractor ECU disabled in settings, skipping ECU initialization." << std::endl; - } - else - { - log("Warning") << "TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; - } + log("Warning") << "TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; } } @@ -1835,8 +1836,8 @@ void Application::update_vt_status_strings(bool aogConnected) ConfigNmeaSendLabel, // pool length 14 nmea2000MessageInterface ? (std::string("NMEA Send: ") + (settings->is_nmea_send_enabled() ? "ON" : "OFF")) : "NMEA Send: N/A"); send_vt_string_if_changed( - ConfigTecuLabel, - std::string("TECU: ") + (settings->is_tecu_enabled() ? "ON" : "OFF") + " (restart req)"); + ConfigTecuLabel, // pool length 23. Off: the tractor's TECU leads and ours only fills its gaps + settings->is_tecu_enabled() ? "TECU: AOG (restart req)" : "TECU: tractor (restart)"); std::ostringstream networkRows; std::uint8_t displayedCFs = 0; From ae37e999a71b0a1d0b5bea61242f27fb28394e51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 19:34:56 +0100 Subject: [PATCH 15/22] Bump AgIsoStack++ to upstream main 02f3fe9 Picks up the upstream fixes since 1d081d9, among them: - #834 take a control function offline only after two missed address claim requests - #857 ignore short TC process data frames and truncated DDOP objects - #818 TC server: width editable flag, working set logging, version request - #798 VT client terminate/receive safe against a restart from another thread - #808 send a TP session's first frame once when sending from another thread - #781 bounds-check VT Change String Value, log the VT's error response - #724 keep a new partner's address claim credit when it replaces a CF - #789 Windows WinUSB driver for candleLight/gs_usb adapters (not enabled here) No TC code changes needed. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 167ea70..46b8db9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,7 +62,7 @@ FetchContent_MakeAvailable(Boost) FetchContent_Declare( isobus GIT_REPOSITORY https://github.com/Open-Agriculture/AgIsoStack-plus-plus.git - GIT_TAG 1d081d9a9df61e256015efda3683d15c8fa3d580 + GIT_TAG 02f3fe910de5fdc73e2014394c2e80900490b9cc DOWNLOAD_EXTRACT_TIMESTAMP TRUE) FetchContent_MakeAvailable(isobus) From 1b044976863cfb5d28685b24c3ecc1a4bab92d39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 19:39:42 +0100 Subject: [PATCH 16/22] Add the candleLight/gs_usb adapter option on Windows AgIsoStack++ #789 added a WinUSB driver for adapters running candleLight (gs_usb) firmware, such as CANable 2 and the WeAct USB2CANFD. Build it into the Windows TC and select it with --can_adapter=candlelight (or gs_usb). --can_channel picks the adapter when several are plugged in (1 = first, empty = 1). It uses Windows' own WinUSB, so no vendor DLL ships with it. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 1 + docs/PROTOCOL.md | 4 ++-- src/main.cpp | 14 +++++++++++++- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 46b8db9..6f73957 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ if(NOT DEFINED CAN_DRIVER) list(APPEND CAN_DRIVER "WindowsInnoMakerUSB2CAN") list(APPEND CAN_DRIVER "TouCAN") list(APPEND CAN_DRIVER "SYS_TEC") + list(APPEND CAN_DRIVER "WindowsCandleLight") elseif(APPLE) set(CAN_DRIVER "MacCANPCAN") else() diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 27478d4..b14cafa 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -239,8 +239,8 @@ AOG-TaskController [options] |---|---|---| | `--help` | — | Print usage and exit. | | `--version` | — | Print git-describe version (with `-dirty` suffix on a dirty tree) and exit. | -| `--can_adapter=` | none | CAN driver. One of: `peak-pcan`, `innomaker-usb2can`, `rusoku-toucan`, `sys-tec-usb2can` (all Windows), `socketcan` (Linux). **Required** — the TC will not start without a driver. | -| `--can_channel=` | — | Driver-specific channel. Numeric (`1`, `2`, ...) for Windows USB adapters; interface name (`can0`, `vcan0`) for SocketCAN. | +| `--can_adapter=` | none | CAN driver. One of: `peak-pcan`, `innomaker-usb2can`, `rusoku-toucan`, `sys-tec-usb2can`, `candlelight` (alias `gs_usb`; candleLight/gs_usb firmware over WinUSB) (all Windows), `socketcan` (Linux). **Required** — the TC will not start without a driver. | +| `--can_channel=` | — | Driver-specific channel. Numeric (`1`, `2`, ...) for Windows USB adapters (for `candlelight`, which adapter when several are plugged in; empty = `1`); interface name (`can0`, `vcan0`) for SocketCAN. | | `--log_level=` | — | One of `debug`, `info`, `warning`, `error`, `critical`. Filters AgIsoStack log output. | | `--log2file` | off | Also write all output to `/logs/AOG-TaskController_YYYY-M-D_H-M.log`. | diff --git a/src/main.cpp b/src/main.cpp index 3cf00a6..a22df3a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -104,6 +104,7 @@ enum class CANAdapter ADAPTER_INNOMAKER_USB2CAN, ADAPTER_RUSOKU_TOUCAN, ADAPTER_SYS_TEC_USB2CAN, + ADAPTER_CANDLELIGHT, ADAPTER_SOCKETCAN, }; @@ -150,7 +151,7 @@ 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 << " --can_adapter=\tSelect the CAN driver (peak-pcan, innomaker-usb2can, rusoku-toucan, sys-tec-usb2can, socketcan)\n"; + std::cout << " --can_adapter=\tSelect the CAN driver (peak-pcan, innomaker-usb2can, rusoku-toucan, sys-tec-usb2can, candlelight, socketcan)\n"; std::cout << " --can_channel=\tSelect the CAN channel (numeric, or interface name e.g. can0 for socketcan)\n"; std::cout << " --log_level=\tSet the log level (debug, info, warning, error, critical)\n"; std::cout << " --log2file\t\tLog to file\n"; @@ -189,6 +190,8 @@ class ArgumentProcessor { "innomaker-usb2can", CANAdapter::ADAPTER_INNOMAKER_USB2CAN }, { "rusoku-toucan", CANAdapter::ADAPTER_RUSOKU_TOUCAN }, { "sys-tec-usb2can", CANAdapter::ADAPTER_SYS_TEC_USB2CAN }, + { "candlelight", CANAdapter::ADAPTER_CANDLELIGHT }, + { "gs_usb", CANAdapter::ADAPTER_CANDLELIGHT }, { "socketcan", CANAdapter::ADAPTER_SOCKETCAN }, }; @@ -269,6 +272,15 @@ static std::shared_ptr create_can_driver(const Argume return std::make_shared(static_cast(std::stoi(args.get_can_channel()))); } #endif +#if defined(ISOBUS_WINDOWSCANDLELIGHT_AVAILABLE) + case CANAdapter::ADAPTER_CANDLELIGHT: + { + // candleLight/gs_usb firmware over WinUSB. The channel picks the adapter, 1 = first one found + const std::string channel = args.get_can_channel(); + const int adapterNumber = channel.empty() ? 1 : std::stoi(channel); + return std::make_shared(static_cast(std::max(adapterNumber, 1) - 1)); + } +#endif #if defined(ISOBUS_SOCKETCAN_AVAILABLE) case CANAdapter::ADAPTER_SOCKETCAN: { From f5ed4050e854241b5a4be966614ac3eade9029b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 19:47:33 +0100 Subject: [PATCH 17/22] Make the TC number configurable (tcNumber setting) The TC number is the TC NAME's function instance + 1 (ISO 11783-10), and was fixed at TC #1. Add a tcNumber setting (1-32, default 1) so the TC can run as TC #2 or higher next to a tractor's own TC. Out-of-range values fall back to 1. It is part of the NAME, so it takes effect after a restart. The TECU keeps function instance 0. Co-Authored-By: Claude Opus 5.5 --- docs/LINUX_DAEMON.md | 2 ++ docs/PROTOCOL.md | 2 ++ include/settings.hpp | 17 ++++++++++++++++ readme.md | 2 ++ src/app.cpp | 6 ++++-- src/settings.cpp | 47 ++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 74 insertions(+), 2 deletions(-) diff --git a/docs/LINUX_DAEMON.md b/docs/LINUX_DAEMON.md index 469b8d7..19af3a1 100644 --- a/docs/LINUX_DAEMON.md +++ b/docs/LINUX_DAEMON.md @@ -125,6 +125,7 @@ The TC reads `settings.json` from `$XDG_CONFIG_HOME/AOG-TaskController/` (typica "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } @@ -138,6 +139,7 @@ The TC reads `settings.json` from `$XDG_CONFIG_HOME/AOG-TaskController/` (typica | `aogHeartbeatEnabled` | `bool` | `true` | Whether the TC sends a 100 ms heartbeat UDP packet to AgIO/AgValonia even when no implement is connected. **Set to `false` if pairing with AgOpenGPS pre-v6.8.2 beta 5** — older AOG versions choke on the heartbeat. | | `vtEnabled` | `bool` | `true` | Whether the TC registers its Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`–`4`); `3` selects Second Edition Draft. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character ISOBUS language code. | | `countryCode` | `string` | `"US"` | Two-character ISOBUS country code. | diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 27478d4..acfd5a3 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -209,6 +209,7 @@ The TC reads `settings.json` from a per-user config directory: "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } @@ -222,6 +223,7 @@ The TC reads `settings.json` from a per-user config directory: | `aogHeartbeatEnabled` | `bool` | `true` | Send `0xF0` heartbeat to AgIO/AgValonia every 100 ms even with no implement. Disable for AOG < v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Register the Virtual Terminal client and display the TC UI when a VT is present. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`=DIS, `1`=FDIS.1, `2`=First Edition, `3`=Second Edition Draft, `4`=Second Published Edition). | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised through the ISOBUS language interface. | | `countryCode` | `string` | `"US"` | Two-character country code advertised through the ISOBUS language interface. | diff --git a/include/settings.hpp b/include/settings.hpp index 06f0278..eaf2920 100644 --- a/include/settings.hpp +++ b/include/settings.hpp @@ -112,6 +112,20 @@ class Settings */ bool set_tc_version(std::uint8_t version, bool save = true); + /** + * @brief Get the configured TC number (ISO 11783-10: function instance + 1) + * @return The TC number (1-32, default 1) + */ + std::uint8_t get_tc_number() const; + + /** + * @brief Set the TC number. Takes effect after a restart, since it is part of the TC's NAME. + * @param number The TC number (1-32); out of range sets the default + * @param save Whether or not to save the settings to file + * @return True if the setting was set successfully, false otherwise + */ + bool set_tc_number(std::uint8_t number, bool save = true); + /** * @brief Get the configured language code * @return The language code (default "en") @@ -162,6 +176,8 @@ class Settings constexpr static bool DEFAULT_AOG_HEARTBEAT_ENABLED = true; constexpr static bool DEFAULT_VT_ENABLED = true; constexpr static std::uint8_t DEFAULT_TC_VERSION = 3; ///< SecondEditionDraft (V3 default for maximum implement compatibility) + constexpr static std::uint8_t DEFAULT_TC_NUMBER = 1; ///< TC #1 = function instance 0 + constexpr static std::uint8_t MAX_TC_NUMBER = 32; ///< The NAME function instance has 5 bits (0-31) static const std::string DEFAULT_LANGUAGE_CODE; static const std::string DEFAULT_COUNTRY_CODE; std::array configuredSubnet = DEFAULT_SUBNET; @@ -170,6 +186,7 @@ class Settings bool aogHeartbeatEnabled = DEFAULT_AOG_HEARTBEAT_ENABLED; bool vtEnabled = DEFAULT_VT_ENABLED; std::uint8_t tcVersion = DEFAULT_TC_VERSION; + std::uint8_t tcNumber = DEFAULT_TC_NUMBER; std::string languageCode = DEFAULT_LANGUAGE_CODE; std::string countryCode = DEFAULT_COUNTRY_CODE; }; diff --git a/readme.md b/readme.md index 8c67159..f3a851b 100644 --- a/readme.md +++ b/readme.md @@ -65,6 +65,7 @@ AOG-TaskController reads its configuration from a `settings.json` file located i | `aogHeartbeatEnabled` | `bool` | `true` | Sends the 100 ms zero-section heartbeat when no implement is connected. Disable for AgOpenGPS versions before v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Enables the ISOBUS Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code, from `0` through `4`; `3` is Second Edition Draft. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised on ISOBUS. | | `countryCode` | `string` | `"US"` | Two-character country code advertised on ISOBUS. | @@ -78,6 +79,7 @@ AOG-TaskController reads its configuration from a `settings.json` file located i "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } diff --git a/src/app.cpp b/src/app.cpp index a697e8b..bfdab94 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -228,19 +228,21 @@ bool Application::setup_control_functions() ourNAME.set_device_class(0); ourNAME.set_identity_number(20); ourNAME.set_ecu_instance(0); - ourNAME.set_function_instance(0); // TC #1. If you want to change the TC number, change this. + ourNAME.set_function_instance(0); ourNAME.set_device_class_instance(0); ourNAME.set_manufacturer_code(1407); isobus::NAME tcNAME = ourNAME; tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); + // ISO 11783-10: the TC number is the TC's NAME function instance + 1 (TC #1 = instance 0) + tcNAME.set_function_instance(static_cast(settings->get_tc_number() - 1)); isobus::NAME tecuNAME = ourNAME; tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed tecuNAME.set_ecu_instance(0); - log("Init") << "Creating Task Controller control function..." << std::endl; + log("Init") << "Creating Task Controller control function (TC number " << static_cast(settings->get_tc_number()) << ")..." << std::endl; tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 // Wait for TC address claim with bounded wait loop (no async to avoid blocking on destruction) diff --git a/src/settings.cpp b/src/settings.cpp index 960927a..56b33e9 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -97,6 +97,27 @@ bool Settings::load() tcVersion = DEFAULT_TC_VERSION; // Key not found, use default } + tcNumber = DEFAULT_TC_NUMBER; + if (data.contains("tcNumber")) + { + try + { + int number = data["tcNumber"].get(); + if (number >= 1 && number <= MAX_TC_NUMBER) + { + tcNumber = static_cast(number); + } + else + { + std::cout << "[" << get_timestamp() << "] Invalid tcNumber " << number << " (1-" << static_cast(MAX_TC_NUMBER) << "), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + } + } + catch (const nlohmann::json::exception &e) + { + std::cout << "[" << get_timestamp() << "] Error parsing 'tcNumber': " << e.what() << std::endl; + } + } + if (data.contains("languageCode")) { try @@ -144,6 +165,7 @@ bool Settings::save() const data["aogHeartbeatEnabled"] = aogHeartbeatEnabled; data["vtEnabled"] = vtEnabled; data["tcVersion"] = tcVersion; + data["tcNumber"] = tcNumber; data["languageCode"] = languageCode; data["countryCode"] = countryCode; @@ -297,6 +319,31 @@ bool Settings::set_tc_version(std::uint8_t version, bool save) return true; } +std::uint8_t Settings::get_tc_number() const +{ + std::scoped_lock lock(settingsMutex); + return tcNumber; +} + +bool Settings::set_tc_number(std::uint8_t number, bool save) +{ + std::scoped_lock lock(settingsMutex); + if ((number < 1) || (number > MAX_TC_NUMBER)) + { + std::cout << "[" << get_timestamp() << "] Invalid TC number " << static_cast(number) << " (1-" << static_cast(MAX_TC_NUMBER) << "), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + tcNumber = DEFAULT_TC_NUMBER; + } + else + { + tcNumber = number; + } + if (save) + { + return this->save(); + } + return true; +} + std::string Settings::get_language_code() const { std::scoped_lock lock(settingsMutex); From 317824fb7214e8bd27e15364dec5e4e859f2a247 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 19:55:04 +0100 Subject: [PATCH 18/22] Send nothing as a secondary TECU until the plan has settled setup_tecu_interfaces() said a secondary waits, but plan_tecu_messages() with no knowledge of the other Tractor ECUs answers "send everything". A secondary doubled the messages of the tractor for up to 2.5 s after claiming an address, which ISO 11783-9 4.4.2.1 c forbids. Start from an empty plan instead. Co-Authored-By: Claude Opus 5.5 --- src/app.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/app.cpp b/src/app.cpp index f2d334f..1221d6d 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -485,7 +485,7 @@ void Application::setup_tecu_interfaces() // The primary sends everything. A secondary sends nothing until it knows what the other // Tractor ECUs provide (update_tecu()), so it never doubles one of their messages. - const TecuMessagePlan plan = plan_tecu_messages(TecuRole::Primary == tecuRole, Facilities{}, {}); + const TecuMessagePlan plan = (TecuRole::Primary == tecuRole) ? plan_tecu_messages(true, Facilities{}, {}) : TecuMessagePlan{}; log("Init") << "Creating Speed Messages Interface on TECU..." << std::endl; create_speed_messages_interface(plan); From 00a05a9af5eaf95858ad766efb3ca0c41a96cf3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 19:58:58 +0100 Subject: [PATCH 19/22] Answer requests for Time/Date (PGN 65254) from the TECU ISO 11783-7 B.1 lists Time/Date as "on request", so the TECU answers requests and never sends it unrequested. The answer is UTC from the PC clock plus the local offset, which now includes daylight saving time. It follows the TECU plan: the primary always answers, and a secondary only when no lower Tractor ECU advertises or sends Time/Date (ISO 11783-9 4.4.2.1). The facility bit in PGN 65033 follows the same state. Time/Date seen from another Tractor ECU counts for 30 s rather than 3 s, because it may only appear on request. The answer lives in TractorFacilities' PGN request handling rather than in TimeDateInterface: that registers a global receive callback, which the stack can still call once after the TECU is rebuilt. Replaces the on-request half of #87, without its 10 s broadcast. Co-Authored-By: Claude Opus 5.5 --- docs/CONCURRENCY.md | 2 + docs/PROTOCOL.md | 7 ++- include/tecu_coexistence.hpp | 4 +- include/tractor_facilities.hpp | 30 ++++++++-- src/app.cpp | 2 + src/tecu_coexistence.cpp | 3 +- src/tractor_facilities.cpp | 98 ++++++++++++++++++++++++++++++- tools/test_tractor_facilities.cpp | 57 ++++++++++++++++++ 8 files changed, 189 insertions(+), 14 deletions(-) diff --git a/docs/CONCURRENCY.md b/docs/CONCURRENCY.md index dcbbb3e..3e53ed9 100644 --- a/docs/CONCURRENCY.md +++ b/docs/CONCURRENCY.md @@ -71,6 +71,8 @@ registration point itself defers to an `update()` call, or invokes the listener/ object. `TractorFacilities`' PGN 65032 callback may still run once after removal, which is harmless: it only logs and never touches the object. `TractorFacilities::providedFacilities` is under its own mutex (set on the main thread, read when answering a PGN 65033 request). +- **`TractorFacilities::timeDateActive`** — atomic: set on the main thread from the TECU plan, read on the + background thread when answering a request for PGN 65033 or PGN 65254. ## What's *not* protected yet — known gap diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 37bc229..8986e5e 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -271,6 +271,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). | | `0xFE48`, `0xFE49`, `0xF022` (PGN 65096, 65097, 61474) | 100 ms | TECU | Wheel-based speed, ground-based speed, machine selected speed (with distance and direction). As a secondary, only those no other Tractor ECU provides (§5.7). | | `0xFE09` (PGN 65033 Tractor Facilities) | Power-up + on request, and when it changes | TECU | 8-byte facility bitmask advertising which PGNs the TECU actually broadcasts. See §5.6. | +| `0xFEE6` (PGN 65254 Time/Date) | On request only (ISO 11783-7 B.1) | TECU | UTC from the PC clock plus the local offset, including daylight saving time. A secondary TECU leaves requests to a Tractor ECU that provides it. | | NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. Not while another Tractor ECU sends it (§5.7). | | GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | GPS fix quality from AgIO (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` if `0xD6` has never arrived, and to `0` once it stops arriving (see §2.5). | @@ -313,6 +314,7 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 | 1 | 8,7 | TECU class | Always `00` (Class 1). | | 1 | 2 | Ground-based speed (PGN 65097) | The TECU sends it: always as the primary; as a secondary only if no other Tractor ECU provides it (§5.7). | | 1 | 3 | Wheel-based speed (PGN 65096) | Same rule. | +| 3 | 8 | Time/date (PGN 65254) | Our TECU answers requests for it: as the primary always, as a secondary only when no lower Tractor ECU advertises or sends it. | | 3 | 7,6 | Ground-based distance + direction | Same as ground-based speed. | | 3 | 5,4 | Wheel-based distance + direction | Same as wheel-based speed. | | 5 | 3 | Selected speed (machine selected speed, PGN 61474) | Same rule. | @@ -320,7 +322,6 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 **Facilities NOT advertised (bits always 0):** - Engine speed — no engine CAN access. -- Time/date (PGN 65254) — the TC does not broadcast it. - Power management — no key switch or power timer signals. - Hitch position / in-work / draft — the hydraulic lift output is a command we issue, not measured feedback; implements would trust it for work-state logic. - PTO shaft speed / engagement — no PTO sensor. @@ -331,7 +332,7 @@ When the TECU is enabled, the TC responds to PGN 65033 requests (ISO 11783-7 B.2 - Front hitch / PTO — no front hitch or PTO sensors. - All reserved bits (byte 2 bits 2–1, byte 4 bits 3–1, byte 5 bit 5, byte 7, byte 8 including the reserved-bit indicator at bit 1). -**Default payload** (TECU enabled as the primary): `[0x06, 0x00, 0x78, 0x00, 0x04, 0x00, 0x00, 0x00]`. +**Default payload** (TECU enabled as the primary): `[0x06, 0x00, 0xF8, 0x00, 0x04, 0x00, 0x00, 0x00]`. **PGN 65032 (Required Tractor Facilities):** When an implement broadcasts what it needs, the TC logs the request at debug level. The response is not modified based on the implement's requirements — a facility bit is set to 1 only when backed by a live broadcast. @@ -342,7 +343,7 @@ ISO 11783-9:2012 §4.4.2.1 allows several Tractor ECUs on the implement bus. The - **`tecuEnabled` picks the role.** `true`: AOG is the tractor's TECU. At startup the TC's own address claim makes every control function on the bus announce itself; if no other Tractor ECU (function code 134) is there, our TECU claims `240` as the primary (function instance 0) and sends everything in §5.2, otherwise it starts as a secondary. `false` ("the tractor has its own TECU"): always a secondary at an arbitrary address, with a function instance above every Tractor ECU on the bus. With no other Tractor ECU there, every message is a gap and it sends them all; once the tractor's appears, it cuts back to what that one lacks. - **Never taking `240` from a tractor:** our TECU NAME is arbitrary-address-capable in both roles. That is the most significant NAME bit, so in an address claim contest for `240` our primary loses to a fixed-address Tractor ECU and moves elsewhere, instead of knocking the tractor's ECU off the bus. - **Stepping down at runtime** (`tecuEnabled: true`): once a second the TC checks for other Tractor ECUs. If another one with function instance 0 appears while ours is the primary, or ours lost `240`, our TECU is rebuilt as a secondary above every Tractor ECU's function instance. Another secondary appearing is no reason to step down. The operator sees a hardware message ("Tractor ECU found: AOG adds only missing data"). -- **What a secondary sends:** nothing until the other Tractor ECUs have answered PGN 65033 (and at least 1 s has passed) or 2.5 s have passed. Then each of ground-based speed, wheel-based speed, machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG is sent only if no Tractor ECU with a lower function instance advertises it in PGN 65033 *and* none has sent it in the last 3 s. Only lower instances count, so two secondaries never wait on each other. Its PGN 65033 advertises exactly what it sends, and is re-sent when that changes. +- **What a secondary sends:** nothing until the other Tractor ECUs have answered PGN 65033 (and at least 1 s has passed) or 2.5 s have passed. Then each of ground-based speed, wheel-based speed, machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG is sent only if no Tractor ECU with a lower function instance advertises it in PGN 65033 *and* none has sent it in the last 3 s. Answering requests for time/date follows the same rule with a 30 s window, because another Tractor ECU may only send it when asked. Only lower instances count, so two secondaries never wait on each other. Its PGN 65033 advertises exactly what it sends, and is re-sent when that changes. - A secondary does not step back up to primary if the tractor's Tractor ECU leaves; it then sends everything, still as a secondary. --- diff --git a/include/tecu_coexistence.hpp b/include/tecu_coexistence.hpp index 99fcacb..ee0226c 100644 --- a/include/tecu_coexistence.hpp +++ b/include/tecu_coexistence.hpp @@ -110,6 +110,8 @@ class OtherTecuMonitor public: /// How long a message seen from another TECU counts as "sent by it" static constexpr std::uint32_t SENT_BY_OTHERS_WINDOW_MS = 3000; + /// The same for time/date, which may only appear when something requests it + static constexpr std::uint32_t TIME_DATE_SENT_BY_OTHERS_WINDOW_MS = 30000; /// @brief Register the PGN 65033 listener. Call once. void initialize(); @@ -130,7 +132,7 @@ class OtherTecuMonitor /// @brief Whether every other TECU has answered with PGN 65033 bool all_facilities_received() const; - /// @brief Messages seen from another TECU within SENT_BY_OTHERS_WINDOW_MS + /// @brief Messages seen from another TECU within SENT_BY_OTHERS_WINDOW_MS (time/date: its own window) std::array sent_by_others() const; /// @brief Whether we already asked this address for PGN 65033 since it appeared diff --git a/include/tractor_facilities.hpp b/include/tractor_facilities.hpp index 28f556d..34b14c7 100644 --- a/include/tractor_facilities.hpp +++ b/include/tractor_facilities.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -94,6 +95,14 @@ std::array encode_facilities(const Facilities &f); /// @brief Decode an 8-byte PGN 65033 payload into a Facilities struct. Facilities decode_facilities(const std::array &payload); +/// @brief The minutes to add to UTC to get local time, positive east of Greenwich. +/// Compares the two broken-down clocks, so it includes daylight saving time and needs no time zone lookup. +int utc_offset_minutes(const std::tm &utc, const std::tm &local); + +/// @brief Encode the Time/Date payload (PGN 65254, ISO 11783-7 B.1) from the same instant as UTC and local time. +/// @param milliseconds Milliseconds into the current second; the message has a resolution of 250 ms. +std::array encode_time_date(const std::tm &utc, const std::tm &local, int milliseconds); + /// @brief Implements ISO 11783-7 Tractor Facilities response (PGN 65033) /// and Required Tractor Facilities diagnostic logging (PGN 65032). class TractorFacilities @@ -101,6 +110,7 @@ class TractorFacilities public: static constexpr std::uint32_t PGN_TRACTOR_FACILITIES = 65033; ///< 0xFE09 static constexpr std::uint32_t PGN_REQUIRED_TRACTOR_FACILITIES = 65032; ///< 0xFE08 + static constexpr std::uint32_t PGN_TIME_DATE = 65254; ///< 0xFEE6 TractorFacilities(std::shared_ptr tecuCF, std::shared_ptr settings); @@ -129,8 +139,9 @@ class TractorFacilities /// @brief Provide a pointer to the NMEA 2000 message interface. void set_nmea2000_message_interface(isobus::NMEA2000MessageInterface *iface); - /// @brief Set whether time/date (PGN 65254 / FEE6) is being broadcast. - /// When true, the time/date facility bit is advertised in PGN 65033. + /// @brief Set whether our TECU provides time/date (PGN 65254 / FEE6). + /// When true, it answers requests for that PGN and advertises the time/date facility bit in PGN 65033. + /// When false, it leaves requests to the other Tractor ECU. It is never sent unrequested (ISO 11783-7 B.1). void set_time_date_active(bool active); /// @brief Advertise exactly these facilities (the time/date bit still follows set_time_date_active()). @@ -149,6 +160,13 @@ class TractorFacilities isobus::AcknowledgementType &acknowledgeType, void *parentPointer); + /// @brief Static PGN-request callback registered for PGN 65254 (Time/Date). + static bool on_time_date_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + isobus::AcknowledgementType &acknowledgeType, + void *parentPointer); + /// @brief Static global-PGN callback for PGN 65032 (Required Tractor /// Facilities). Diagnostic only — logs the request unconditionally /// (not gated by log level) and never changes our response. @@ -158,11 +176,11 @@ class TractorFacilities std::shared_ptr settings; isobus::SpeedMessagesInterface *speedMessagesInterface = nullptr; isobus::NMEA2000MessageInterface *nmea2000MessageInterface = nullptr; - /// Whether we are broadcasting PGN 65254 (FEE6). Atomic because - /// set_time_date_active() and build_payload() are called from different - /// isobus callback contexts (see docs/CONCURRENCY.md) that can run - /// concurrently on different threads. + /// Whether we provide PGN 65254 (FEE6). Atomic because set_time_date_active() + /// runs on the main thread, while the PGN request callbacks that read it run + /// on the CAN stack's thread (see docs/CONCURRENCY.md). std::atomic timeDateActive{ false }; + std::atomic timeDateRequestLogged{ false }; ///< To log only the first request answered /// Set by set_provided_facilities() on the main thread, read by build_payload() from PGN request callbacks mutable std::mutex providedFacilitiesMutex; diff --git a/src/app.cpp b/src/app.cpp index 1221d6d..9d3c023 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -502,6 +502,7 @@ void Application::setup_tecu_interfaces() tractorFacilities->set_speed_messages_interface(speedMessagesInterface.get()); tractorFacilities->set_nmea2000_message_interface(nmea2000MessageInterface.get()); tractorFacilities->set_provided_facilities(facilities_for_plan(plan)); + tractorFacilities->set_time_date_active(plan.sends(TecuMessage::TimeDate)); tractorFacilities->initialize(); tecuPlan = plan; tecuInterfacesReady = true; @@ -636,6 +637,7 @@ void Application::apply_tecu_plan(const TecuMessagePlan &plan) if (tractorFacilities) { tractorFacilities->set_provided_facilities(facilities_for_plan(plan)); + tractorFacilities->set_time_date_active(plan.sends(TecuMessage::TimeDate)); if (tractorFacilitiesSentOnPowerUp) { // Implements that cached our earlier response learn what changed diff --git a/src/tecu_coexistence.cpp b/src/tecu_coexistence.cpp index 11811e9..936b8e6 100644 --- a/src/tecu_coexistence.cpp +++ b/src/tecu_coexistence.cpp @@ -274,7 +274,8 @@ std::array OtherTecuMonitor::sent_by_others() const for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) { const std::uint32_t last = lastSentByOthersMs[i]; - sent[i] = (0 != last) && !isobus::SystemTiming::time_expired_ms(last, SENT_BY_OTHERS_WINDOW_MS); + const bool isTimeDate = (static_cast(TecuMessage::TimeDate) == i); + sent[i] = (0 != last) && !isobus::SystemTiming::time_expired_ms(last, isTimeDate ? TIME_DATE_SENT_BY_OTHERS_WINDOW_MS : SENT_BY_OTHERS_WINDOW_MS); } return sent; } diff --git a/src/tractor_facilities.cpp b/src/tractor_facilities.cpp index ed80093..70390f6 100644 --- a/src/tractor_facilities.cpp +++ b/src/tractor_facilities.cpp @@ -14,6 +14,8 @@ #include "logging_utils.hpp" #include "settings.hpp" +#include +#include #include #include #include @@ -242,6 +244,63 @@ Facilities decode_facilities(const std::array &p) return f; } +// --------------------------------------------------------------------------- +// Time/Date +// --------------------------------------------------------------------------- + +int utc_offset_minutes(const std::tm &utc, const std::tm &local) +{ + // The two clocks are at most a day apart. Across New Year tm_yday jumps, so compare the years first. + int dayDifference = local.tm_yday - utc.tm_yday; + if (local.tm_year != utc.tm_year) + { + dayDifference = (local.tm_year > utc.tm_year) ? 1 : -1; + } + return (dayDifference * 24 * 60) + ((local.tm_hour - utc.tm_hour) * 60) + (local.tm_min - utc.tm_min); +} + +std::array encode_time_date(const std::tm &utc, const std::tm &local, int milliseconds) +{ + const int offsetMinutes = utc_offset_minutes(utc, local); + const int seconds = std::min(utc.tm_sec, 59); // tm_sec is 60 during a leap second + const int quarterOfDay = utc.tm_hour / 6; + + std::array p{}; + p[0] = static_cast((seconds * 4) + (milliseconds / 250)); // 0.25 s/bit + p[1] = static_cast(utc.tm_min); + p[2] = static_cast(utc.tm_hour); + p[3] = static_cast(utc.tm_mon + 1); + p[4] = static_cast(((utc.tm_mday - 1) * 4) + quarterOfDay + 1); // 0.25 day/bit, 0 = not a day + p[5] = static_cast(utc.tm_year + 1900 - 1985); + // Both offsets carry the sign: -3:30 is -3 h and -30 min + p[6] = static_cast((offsetMinutes % 60) + 125); + p[7] = static_cast((offsetMinutes / 60) + 125); + return p; +} + +/// Read the PC clock. False if it is outside what the message can carry, e.g. a clock that was never set. +static bool read_system_time_date(std::array &payload) +{ + const auto now = std::chrono::system_clock::now(); + const std::time_t seconds = std::chrono::system_clock::to_time_t(now); + const auto milliseconds = std::chrono::duration_cast(now.time_since_epoch()).count() % 1000; + + std::tm utc{}; + std::tm local{}; +#if defined(_WIN32) + const bool converted = (0 == gmtime_s(&utc, &seconds)) && (0 == localtime_s(&local, &seconds)); +#else + const bool converted = (nullptr != gmtime_r(&seconds, &utc)) && (nullptr != localtime_r(&seconds, &local)); +#endif + const int year = utc.tm_year + 1900; + if (!converted || (year < 1985) || (year > 2235)) + { + return false; + } + payload = encode_time_date(utc, local, static_cast(milliseconds)); + return true; +} + // --------------------------------------------------------------------------- // TractorFacilities // --------------------------------------------------------------------------- @@ -269,6 +328,10 @@ TractorFacilities::~TractorFacilities() } isobus::CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback( PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); + if (auto pgnRequestProtocol = tecuCF ? tecuCF->get_pgn_request_protocol().lock() : nullptr) + { + pgnRequestProtocol->remove_pgn_request_callback(PGN_TIME_DATE, &TractorFacilities::on_time_date_request, this); + } } void TractorFacilities::set_provided_facilities(const Facilities &facilities) @@ -322,9 +385,8 @@ std::array TractorFacilities::build_payload() const f.wheelBasedDistance = wheelSpeedActive; f.wheelBasedDirection = wheelSpeedActive; - // Time/date (PGN 65254 / FEE6) – set by the application layer when - // the TECU is actively broadcasting FEE6 and no duplicate provider - // exists on the bus. + // Time/date (PGN 65254 / FEE6) – set by the application layer while + // the TECU answers requests for it. f.timeDate = timeDateActive; // Everything else stays 0: we have no engine data, hitch feedback, @@ -367,6 +429,11 @@ bool TractorFacilities::initialize() PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); std::cout << "[" << get_timestamp() << "] [TractorFacilities] Registered PGN 65032 diagnostic listener." << std::endl; + if (!pgnRequestProtocol->register_pgn_request_callback(PGN_TIME_DATE, &TractorFacilities::on_time_date_request, this)) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Failed to register PGN 65254 request callback." << std::endl; + } + initialized = true; return true; } @@ -406,6 +473,31 @@ bool TractorFacilities::send_facilities_response(bool isPowerUp) // Static callbacks // --------------------------------------------------------------------------- +bool TractorFacilities::on_time_date_request( + std::uint32_t parameterGroupNumber, + std::shared_ptr /*requestingControlFunction*/, + bool &acknowledge, + isobus::AcknowledgementType & /*acknowledgeType*/, + void *parentPointer) +{ + auto *self = static_cast(parentPointer); + std::array payload{}; + // Returning false leaves it to the Tractor ECU that does provide it: the stack ignores a global + // request and NACKs one addressed to us + if ((PGN_TIME_DATE != parameterGroupNumber) || (nullptr == self) || !self->timeDateActive || !read_system_time_date(payload)) + { + return false; + } + + acknowledge = false; // Sending the requested PGN is the response + const bool sent = isobus::CANNetworkManager::CANNetwork.send_can_message(PGN_TIME_DATE, payload.data(), payload.size(), self->tecuCF); + if (sent && !self->timeDateRequestLogged.exchange(true)) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Answering requests for PGN 65254 (Time/Date) from the PC clock." << std::endl; + } + return sent; +} + bool TractorFacilities::on_pgn_request( std::uint32_t parameterGroupNumber, std::shared_ptr requestingControlFunction, diff --git a/tools/test_tractor_facilities.cpp b/tools/test_tractor_facilities.cpp index e88644f..f46868f 100644 --- a/tools/test_tractor_facilities.cpp +++ b/tools/test_tractor_facilities.cpp @@ -323,6 +323,61 @@ static void test_rear_hitch_in_work_not_set() check((payload[1] & 0x40) == 0x00, "rear hitch in work (byte 2 bit 7) = 0 by default"); } +static std::tm make_tm(int year, int month, int day, int yday, int hour, int minute, int second) +{ + std::tm t{}; + t.tm_year = year - 1900; + t.tm_mon = month - 1; + t.tm_mday = day; + t.tm_yday = yday; + t.tm_hour = hour; + t.tm_min = minute; + t.tm_sec = second; + return t; +} + +static void test_utc_offset() +{ + std::printf("[test_utc_offset]\n"); + const auto summerUtc = make_tm(2026, 7, 15, 195, 10, 20, 30); + check(utc_offset_minutes(summerUtc, make_tm(2026, 7, 15, 195, 12, 20, 30)) == 120, "CEST (daylight saving) = +120"); + check(utc_offset_minutes(summerUtc, summerUtc) == 0, "UTC = 0"); + + const auto lateUtc = make_tm(2026, 3, 10, 68, 23, 30, 0); + check(utc_offset_minutes(lateUtc, make_tm(2026, 3, 11, 69, 0, 30, 0)) == 60, "local is already tomorrow"); + const auto earlyUtc = make_tm(2026, 3, 11, 69, 1, 0, 0); + check(utc_offset_minutes(earlyUtc, make_tm(2026, 3, 10, 68, 21, 0, 0)) == -240, "local is still yesterday"); + + const auto newYearEveUtc = make_tm(2026, 12, 31, 364, 23, 30, 0); + check(utc_offset_minutes(newYearEveUtc, make_tm(2027, 1, 1, 0, 0, 30, 0)) == 60, "local is already next year"); + const auto newYearUtc = make_tm(2027, 1, 1, 0, 2, 0, 0); + check(utc_offset_minutes(newYearUtc, make_tm(2026, 12, 31, 364, 22, 30, 0)) == -210, "local is still last year, half-hour zone"); +} + +static void test_time_date_payload() +{ + std::printf("[test_time_date_payload]\n"); + const auto utc = make_tm(2026, 7, 15, 195, 10, 20, 30); + const auto p = encode_time_date(utc, make_tm(2026, 7, 15, 195, 12, 20, 30), 750); + check(p[0] == (30 * 4) + 3, "seconds in 0.25 s steps"); + check(p[1] == 20, "minutes"); + check(p[2] == 10, "hours, UTC"); + check(p[3] == 7, "month"); + check(p[4] == (14 * 4) + 1 + 1, "day in quarter days, second quarter"); + check(p[5] == 41, "years since 1985"); + check(p[6] == 125, "local minute offset 0"); + check(p[7] == 127, "local hour offset +2"); + + const auto newYearUtc = make_tm(2027, 1, 1, 0, 2, 0, 0); + const auto west = encode_time_date(newYearUtc, make_tm(2026, 12, 31, 364, 22, 30, 0), 0); + check(west[6] == 125 - 30, "local minute offset -30"); + check(west[7] == 125 - 3, "local hour offset -3"); + check(west[4] == 1, "first quarter of the first day"); + + const auto leap = encode_time_date(make_tm(2026, 6, 30, 180, 23, 59, 60), make_tm(2026, 6, 30, 180, 23, 59, 60), 999); + check(leap[0] == (59 * 4) + 3, "a leap second stays in range"); +} + int main() { test_default_ground_speed_only(); @@ -333,6 +388,8 @@ int main() test_tecu_class_not_available(); test_zero_payload(); test_rear_hitch_in_work_not_set(); + test_utc_offset(); + test_time_date_payload(); if (failures > 0) { From b066073c224918715ad770878902b30de2e90358 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 20:11:47 +0100 Subject: [PATCH 20/22] Fix what the integration merges left doubled Text merges kept two definitions of send_gnss_quality(), and two PGN 0xF3 branches from #102 and #84, where the second could never run. One branch now registers the field for the guidance track code and applies section control on field open. Co-Authored-By: Claude Opus 5.5 --- src/app.cpp | 15 +-------------- src/task_controller.cpp | 13 ------------- 2 files changed, 1 insertion(+), 27 deletions(-) diff --git a/src/app.cpp b/src/app.cpp index fdeda87..a40feba 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -661,20 +661,6 @@ void Application::setup_udp_connections() on_aog_field_opened("started sending section states (PGN 229)"); } } - else if (pgn == 0xF3) // 243 - Field name, empty when the field is closed - { - lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); - const std::string fieldName(data.begin(), data.end()); - if (fieldName.empty()) - { - log("Field") << "AOG closed the field" << std::endl; - } - else - { - log("Field") << "AOG opened field '" << fieldName << "'" << std::endl; - on_aog_field_opened("opened field '" + fieldName + "'"); - } - } else if (pgn == 0xF1 && !data.empty()) // 241 - Section Control { lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); @@ -730,6 +716,7 @@ void Application::setup_udp_connections() hasActiveField = true; log("Field") << "Field opened: " << currentFieldName << " (index " << currentFieldIndex << ")" << std::endl; } + on_aog_field_opened("opened field '" + fieldName + "'"); } } else if (pgn == 0xF4) // 244 - Guidance Track Context diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 045ff47..5ed78b9 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -1227,19 +1227,6 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality) } } -void MyTCServer::send_gnss_quality(std::uint8_t quality) -{ - std::lock_guard lock(clientsMutex); - constexpr auto DDI = isobus::DataDescriptionIndex::GNSSQuality; - for (auto &client : clients) - { - if (client.second.has_element_number_for_ddi(DDI)) - { - send_set_value(client.first, static_cast(DDI), client.second.get_element_number_for_ddi(DDI), static_cast(quality)); - } - } -} - void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm) { if (!ctx.valid) From 1e31bde9425dcc8ea78c23edb315edb1400fbd7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sun, 8 Nov 2026 20:14:55 +0100 Subject: [PATCH 21/22] Keep the field name branch where the guidance track code adds its own PR #84 also adds a PGN 0xF3 branch to this if/else chain, between 0xF1 and 0xF2. In different places, the two merge without a conflict and the second never runs. In the same place, Git reports the conflict. No behaviour change. Co-Authored-By: Claude Opus 5.5 --- src/app.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/app.cpp b/src/app.cpp index a264eab..4ce0397 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -537,6 +537,13 @@ void Application::setup_udp_connections() on_aog_field_opened("started sending section states (PGN 229)"); } } + else if (pgn == 0xF1 && !data.empty()) // 241 - Section Control + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + 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); + } else if (pgn == 0xF3) // 243 - Field name, empty when the field is closed { lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); @@ -551,13 +558,6 @@ void Application::setup_udp_connections() on_aog_field_opened("opened field '" + fieldName + "'"); } } - else if (pgn == 0xF1 && !data.empty()) // 241 - Section Control - { - lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); - 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); - } else if (pgn == 0xF2 && data.size() >= 6) // Process Data { lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); From cda4137c4dd451d0db869e4342c76a6d6642dfd5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Mon, 9 Nov 2026 01:04:12 +0100 Subject: [PATCH 22/22] Let tcNumber 0 pick a free TC number at startup Asks the bus for address claims, then uses the first number no other TC has. It's a setting rather than a command-line option so that it also works when AgIO starts the TC. The claims are read from the received frames, because the stack's roll-call after our request can mark a CF offline that has just answered. Based on the automatic selection in Flodu81's #126, which listened without asking and so could not see a TC that had already claimed its address. Co-authored-by: Flodu81 <129336198+Flodu81@users.noreply.github.com> Co-Authored-By: Claude Opus 5.5 --- docs/LINUX_DAEMON.md | 2 +- docs/PROTOCOL.md | 2 +- include/settings.hpp | 4 +++ readme.md | 2 +- src/app.cpp | 83 ++++++++++++++++++++++++++++++++++++++++++-- src/settings.cpp | 13 ++++--- 6 files changed, 96 insertions(+), 10 deletions(-) diff --git a/docs/LINUX_DAEMON.md b/docs/LINUX_DAEMON.md index 19af3a1..94f8ba9 100644 --- a/docs/LINUX_DAEMON.md +++ b/docs/LINUX_DAEMON.md @@ -139,7 +139,7 @@ The TC reads `settings.json` from `$XDG_CONFIG_HOME/AOG-TaskController/` (typica | `aogHeartbeatEnabled` | `bool` | `true` | Whether the TC sends a 100 ms heartbeat UDP packet to AgIO/AgValonia even when no implement is connected. **Set to `false` if pairing with AgOpenGPS pre-v6.8.2 beta 5** — older AOG versions choke on the heartbeat. | | `vtEnabled` | `bool` | `true` | Whether the TC registers its Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`–`4`); `3` selects Second Edition Draft. | -| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character ISOBUS language code. | | `countryCode` | `string` | `"US"` | Two-character ISOBUS country code. | diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index acfd5a3..b2ffc50 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -223,7 +223,7 @@ The TC reads `settings.json` from a per-user config directory: | `aogHeartbeatEnabled` | `bool` | `true` | Send `0xF0` heartbeat to AgIO/AgValonia every 100 ms even with no implement. Disable for AOG < v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Register the Virtual Terminal client and display the TC UI when a VT is present. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`=DIS, `1`=FDIS.1, `2`=First Edition, `3`=Second Edition Draft, `4`=Second Published Edition). | -| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised through the ISOBUS language interface. | | `countryCode` | `string` | `"US"` | Two-character country code advertised through the ISOBUS language interface. | diff --git a/include/settings.hpp b/include/settings.hpp index eaf2920..a718e3d 100644 --- a/include/settings.hpp +++ b/include/settings.hpp @@ -118,6 +118,9 @@ class Settings */ std::uint8_t get_tc_number() const; + /// @brief Whether the TC number is picked at startup from the numbers free on the bus (tcNumber 0) + bool is_tc_number_automatic() const; + /** * @brief Set the TC number. Takes effect after a restart, since it is part of the TC's NAME. * @param number The TC number (1-32); out of range sets the default @@ -177,6 +180,7 @@ class Settings constexpr static bool DEFAULT_VT_ENABLED = true; constexpr static std::uint8_t DEFAULT_TC_VERSION = 3; ///< SecondEditionDraft (V3 default for maximum implement compatibility) constexpr static std::uint8_t DEFAULT_TC_NUMBER = 1; ///< TC #1 = function instance 0 + constexpr static std::uint8_t TC_NUMBER_AUTOMATIC = 0; ///< Use the first number no other TC on the bus has constexpr static std::uint8_t MAX_TC_NUMBER = 32; ///< The NAME function instance has 5 bits (0-31) static const std::string DEFAULT_LANGUAGE_CODE; static const std::string DEFAULT_COUNTRY_CODE; diff --git a/readme.md b/readme.md index f3a851b..17bbfff 100644 --- a/readme.md +++ b/readme.md @@ -65,7 +65,7 @@ AOG-TaskController reads its configuration from a `settings.json` file located i | `aogHeartbeatEnabled` | `bool` | `true` | Sends the 100 ms zero-section heartbeat when no implement is connected. Disable for AgOpenGPS versions before v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Enables the ISOBUS Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code, from `0` through `4`; `3` is Second Edition Draft. | -| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. Takes effect after restart. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised on ISOBUS. | | `countryCode` | `string` | `"US"` | Two-character country code advertised on ISOBUS. | diff --git a/src/app.cpp b/src/app.cpp index bfdab94..fdc9519 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -27,8 +27,11 @@ #include "logging_utils.hpp" #include +#include +#include #include #include +#include #include #include #include @@ -152,6 +155,50 @@ static bool check_tc_address_conflict(const std::shared_ptr tcInstancesSeen{}; + +// Reads the claims straight from the frames. The stack's table isn't enough: its roll-call after our +// request can mark a CF offline that has just answered, depending on thread timing. +static void record_tc_claim(const isobus::CANMessageFrame &frame) +{ + if ((8 != frame.dataLength) || (static_cast(isobus::CANLibParameterGroupNumber::AddressClaim) != isobus::CANIdentifier(frame.identifier).get_parameter_group_number())) + { + return; + } + + std::uint64_t rawName = 0; + for (std::size_t i = 0; i < 8; i++) + { + rawName |= static_cast(frame.data[i]) << (8 * i); + } + + const isobus::NAME name(rawName); + if ((static_cast(isobus::NAME::Function::TaskController) == name.get_function_code()) && (name.get_function_instance() < tcInstancesSeen.size())) + { + tcInstancesSeen[name.get_function_instance()] = true; + } +} + +// Lowest function instance not claimed by a TC during discovery, if there is one. +static std::optional find_free_tc_function_instance() +{ + std::optional freeInstance; + for (std::uint8_t instance = 0; instance < tcInstancesSeen.size(); instance++) + { + if (tcInstancesSeen[instance]) + { + log("TC Discovery") << "Existing Task Controller with function instance " << static_cast(instance) << std::endl; + } + else if (!freeInstance) + { + freeInstance = instance; + } + } + return freeInstance; +} + Application::Application(std::shared_ptr canDriver) : canDriver(canDriver) { @@ -204,14 +251,32 @@ bool Application::setup_can_hardware() isobus::CANNetworkManager::CANNetwork.get_configuration().set_number_of_packets_per_cts_message(255); + // A TC that claimed its address before we started says nothing about its NAME unless asked + const bool discoverTcNumbers = settings->is_tc_number_automatic(); + isobus::EventCallbackHandle tcClaimListener = 0; + if (discoverTcNumbers) + { + tcClaimListener = isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener(record_tc_claim); + } + if (discoverTcNumbers && !isobus::CANNetworkManager::CANNetwork.send_request_for_address_claim(0)) + { + log("TC Discovery") << "Could not request address claims, TCs already on the bus may be missed" << std::endl; + } + // Start CAN network and allow a brief update window to observe bus CFs + const std::uint32_t busDiscoveryWindowMs = discoverTcNumbers ? 1500 : 100; auto busDiscoveryWindowStart = isobus::SystemTiming::get_timestamp_ms(); - while (isobus::SystemTiming::get_time_elapsed_ms(busDiscoveryWindowStart) < 100) + while (isobus::SystemTiming::get_time_elapsed_ms(busDiscoveryWindowStart) < busDiscoveryWindowMs) { isobus::CANNetworkManager::CANNetwork.update(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + if (discoverTcNumbers) + { + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().remove_listener(tcClaimListener); + } + return true; } @@ -235,14 +300,26 @@ bool Application::setup_control_functions() isobus::NAME tcNAME = ourNAME; tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); // ISO 11783-10: the TC number is the TC's NAME function instance + 1 (TC #1 = instance 0) - tcNAME.set_function_instance(static_cast(settings->get_tc_number() - 1)); + std::uint8_t tcNumber = settings->get_tc_number(); + if (settings->is_tc_number_automatic()) + { + const auto freeInstance = find_free_tc_function_instance(); + if (!freeInstance) + { + log("TC Config") << "No free TC number available" << std::endl; + return false; + } + tcNumber = static_cast(*freeInstance + 1); + log("TC Config") << "Automatically selected TC number " << static_cast(tcNumber) << std::endl; + } + tcNAME.set_function_instance(static_cast(tcNumber - 1)); isobus::NAME tecuNAME = ourNAME; tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed tecuNAME.set_ecu_instance(0); - log("Init") << "Creating Task Controller control function (TC number " << static_cast(settings->get_tc_number()) << ")..." << std::endl; + log("Init") << "Creating Task Controller control function (TC number " << static_cast(tcNumber) << ")..." << std::endl; tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 // Wait for TC address claim with bounded wait loop (no async to avoid blocking on destruction) diff --git a/src/settings.cpp b/src/settings.cpp index 56b33e9..3987c2b 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -103,13 +103,13 @@ bool Settings::load() try { int number = data["tcNumber"].get(); - if (number >= 1 && number <= MAX_TC_NUMBER) + if (number >= TC_NUMBER_AUTOMATIC && number <= MAX_TC_NUMBER) { tcNumber = static_cast(number); } else { - std::cout << "[" << get_timestamp() << "] Invalid tcNumber " << number << " (1-" << static_cast(MAX_TC_NUMBER) << "), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + std::cout << "[" << get_timestamp() << "] Invalid tcNumber " << number << " (1-" << static_cast(MAX_TC_NUMBER) << ", or 0 for automatic), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; } } catch (const nlohmann::json::exception &e) @@ -325,12 +325,17 @@ std::uint8_t Settings::get_tc_number() const return tcNumber; } +bool Settings::is_tc_number_automatic() const +{ + return TC_NUMBER_AUTOMATIC == get_tc_number(); +} + bool Settings::set_tc_number(std::uint8_t number, bool save) { std::scoped_lock lock(settingsMutex); - if ((number < 1) || (number > MAX_TC_NUMBER)) + if (number > MAX_TC_NUMBER) { - std::cout << "[" << get_timestamp() << "] Invalid TC number " << static_cast(number) << " (1-" << static_cast(MAX_TC_NUMBER) << "), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + std::cout << "[" << get_timestamp() << "] Invalid TC number " << static_cast(number) << " (1-" << static_cast(MAX_TC_NUMBER) << ", or 0 for automatic), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; tcNumber = DEFAULT_TC_NUMBER; } else