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 6785346..b722b39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,7 +62,7 @@ FetchContent_MakeAvailable(Boost) FetchContent_Declare( isobus GIT_REPOSITORY https://github.com/gunicsba/AgIsoStack-plus-plus.git - GIT_TAG d4d36dd6f15af6c6f6218317572025a1ffec6883 + GIT_TAG e79699e9f58b332d2c1146ebeb5e25c8908c0e35 DOWNLOAD_EXTRACT_TIMESTAMP TRUE) FetchContent_MakeAvailable(isobus) @@ -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..ea87902 100644 --- a/docs/CONCURRENCY.md +++ b/docs/CONCURRENCY.md @@ -15,9 +15,9 @@ this repo opted into. That thread calls `CANNetworkManager::CANNetwork.update()` is what actually parses incoming CAN frames, handles address claims, and dispatches every PGN to whatever is registered to receive it. -Meanwhile, `Application::update()` — the loop driven from `main.cpp`, doing UDP handling and -periodic sends — runs on the **main thread**. It never calls `CANHardwareInterface::update()` -itself; it relies entirely on that background thread. +Meanwhile, `Application::update()` — the loop driven from `main.cpp`, doing UDP handling, periodic +sends, VT status polling — runs on the **main thread**. It never calls +`CANHardwareInterface::update()` itself; it relies entirely on that background thread. So: **two threads exist for the lifetime of the process**, and the question for any new code is which one it runs on. @@ -25,8 +25,8 @@ which one it runs on. ## Which callbacks run on which thread — checked, not assumed Don't guess this from a class's "manual update mode" comment (that pattern is unrelated to this -question and led to a wrong conclusion once already). Check how the *specific* callback is -registered: +question and led to a wrong conclusion once already — see git history around 2026-09-04). Check +how the *specific* callback is registered: - **Runs on the CAN stack background thread by default** (CANHardwareInterface's updateThread): `TaskControllerServer` (`MyTCServer`). Treat every `TaskControllerServer` override (`activate_object_pool`, @@ -36,10 +36,12 @@ registered: deferred to any `update()` call: `VirtualTerminalClient` (confirmed: `process_rx_message` is registered via `add_global_parameter_group_number_callback` and invokes `softKeyEventDispatcher`/`buttonEventDispatcher`/`changeNumericValueEventDispatcher`/etc. - synchronously). Any raw + synchronously) and `TimeDateInterface` (same pattern — `process_rx_message` invokes + `timeAndDateEventDispatcher.invoke(...)` directly). 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, the FEE6 + duplicate-provider listener, 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 @@ -51,14 +53,23 @@ registration point itself defers to an `update()` call, or invokes the listener/ `MyTCServer::clientsMutex` (a `std::recursive_mutex`, since several methods call each other: e.g. `update_section_states()` → `send_section_setpoint_states()`). Locked at the top of every `TaskControllerServer` override and every method `Application` calls into. `get_clients()` - returns a **copy**, not a reference — call sites hold the result across multiple operations or - take a second `get_clients()` call, either of which would leave a dangling reference to a - temporary if it returned by reference. If you add a new method that reads or writes `clients`, - take `clientsMutex` at its top, even though (per the note above) the `TaskControllerServer` - 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. + returns a **copy**, not a reference — several call sites hold the result across multiple + operations or take a second `get_clients()` call, either of which would leave a dangling + reference to a temporary if it returned by reference. If you add a new method that reads or + writes `clients`, take `clientsMutex` at its top, even though (per the note above) the + `TaskControllerServer` 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. +- **`Application::lastExternalFee6Ms` / `fee6Broadcasting`** (`app.hpp`/`.cpp`) — guarded by + `Application::fee6Mutex`. Written from both the `TimeDateInterface` listener (background thread) + and `Application::update()`'s FEE6 broadcast block (main thread) — confirmed via the + registration-tracing method above. +- **`TractorFacilities::timeDateActive`** (`tractor_facilities.hpp`/`.cpp`) — `std::atomic` + rather than a mutex, since it's a single flag: written by `set_time_date_active()` (called from + both threads, same as above) and read by `build_payload()` (called both on power-up from the main + thread, and from the PGN 65033 request handler on the background thread). -## What's *not* protected yet — known gap +## What's *not* fully protected yet — known gap Calls into `vtClient` (`isobus::VirtualTerminalClient`) happen from both the main thread (`Application::update_vt_client()` and everything it calls: `send_vt_string_if_changed`, @@ -66,14 +77,16 @@ Calls into `vtClient` (`isobus::VirtualTerminalClient`) happen from both the mai `setup_vt_client()` (soft-key, button, numeric-value-change), which — per the confirmed registration pattern above — run on the background thread. Whether `VirtualTerminalClient`'s own internal state is safe under that concurrent access is not verified; no external mutex wraps -`vtClient` usage today. If a crash dump ever points into `isobus::VirtualTerminalClient`, or a -crash correlates with VT/AUX-N activity rather than DDOP upload, this is the first place to look. +`vtClient` usage today. Given the original crash was reported as "the moment a new device (VT +capable) appeared on the bus," this is a credible remaining suspect and should be the first place +to look if a crash dump points into `isobus::VirtualTerminalClient` or if a new crash correlates +with VT/AUX-N activity rather than DDOP upload. ## Also worth knowing: `get_object_by_index()` can return null Unrelated to threading, but found while investigating a real crash report ("usually happens right -after DDOP loading"): `isobus::DeviceDescriptorObjectPool::get_object_by_index()` can return -`nullptr` for an index within `[0, size())` — most likely a symptom of a partially- or +after DDOP loading," predates the VT client entirely): `isobus::DeviceDescriptorObjectPool::get_object_by_index()` +can return `nullptr` for an index within `[0, size())` — most likely a symptom of a partially- or incorrectly-parsed pool (e.g. a multi-chunk DDOP transfer). Two call sites in this file already null-checked it; the rest didn't and were fixed alongside the mutex work above. **Any new call to `get_object_by_index()` must null-check the result before dereferencing.** diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index e9d22e8..9cdcb3e 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. @@ -60,6 +60,7 @@ Source byte identifies the logical sender of a frame. The conventions used today |---|---| | `0x7F` (127) | **AgIO / AgValonia** (the GUI/host application) | | `0x80` (128) | **AOG-TaskController** itself | +| `0x7C` (124) | **AOG's GPS/IMU submodule** — sends PGN `0xD6` only (see §2.5). | Other addresses appear on the ISOBUS side but are not used in UDP frames. @@ -87,14 +88,17 @@ 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 | +| `0xEF` (239) | Machine data | variable | Only used as an AOG-liveness signal today; payload not parsed (see comment in `app.cpp` for the historical byte layout — tram markers there were superseded by PGN `0xF4`). | | `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` | +| `0xF4` (244) | Guidance track context | 10 | Real-time AB-line/track guidance state, feeding ISOBUS TRACK (Tramline Control) — see §5.7. | #### `0xC9` — Subnet detection @@ -102,10 +106,18 @@ 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/Manual/Simulated). This value feeds DDI 514 (GNSSQuality) — see §5.7. Two fallback cases both resolve to `1` (weakest real GNSS fix), not `0` (No GNSS): AOG reporting `6+`, 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, independent of whether AOG overall is still connected). `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). +#### `0xEF` — Machine data + +Historically carried uturn speed, hydraulic lift, geo-stop, and tram-marker bits, consumed by a synthetic guidance-track fallback. That fallback has been removed — guidance now comes exclusively from PGN `0xF4` — so today `0xEF` is only used to update the AOG-liveness timestamp; its payload is not parsed. + #### `0xF1` — Section control mode `1` = automatic (TC drives the implement). `0` = manual (operator drives). The TC logs and propagates this to the implement. @@ -118,10 +130,29 @@ 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 feeds DDI 0x0201 (GuidanceLineDeviation) — see §5.7. | Unknown DDIs are silently ignored (PGN 0xF2 is the generic process-data channel — the TC will gain more DDIs over time). +#### `0xF4` — Guidance track context + +AOG's real-time AB-line/track guidance state, driving ISOBUS TRACK (Tramline Control). 10-byte payload: + +``` + 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) +``` + +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 they reach `GuidanceTrackContext` — confirmed by field testing, not documented anywhere on AOG's side. As sent raw by AOG, the tramline implement's own on-board tram-pattern phase (which pass of N is "on") was consistently one pass out of sync with AOG's own intended tram 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`) in `guidance_track_context.hpp`. + +See §5.7 for how this maps onto the outbound ISOBUS DDIs. + ### 2.6 PGNs outbound (TC → client) All PGNs sent **by the TC to AgIO/AgValonia** use source `0x80`. @@ -250,7 +281,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. | +| `0xFC8E` (Control Function Functionalities) | At claim + periodic | TC | Announces TaskControllerBasicServer (v1), TaskControllerSectionControlServer (v1, 1 boom / 64 sections), and functionality 27 — Tramline/TRACK Server (v1, Level 1). See §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. | +| `0xFEE6` (PGN 65254 Time/Date) | 10 s, suppressed if another provider is detected | TECU | Wall-clock UTC + local offset, from `TimeDateInterface`. Also answers PGN-request for `0xFEE6`. | +| Tramline/TRACK process data (DDI 507-511, 514) | 250 ms, once a client completes negotiation | TC | See §5.7. | | 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,16 +296,18 @@ 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. +- **Tramline/TRACK DDIs (505, 506, 507, 508-511, 515)**: negotiated and tracked per client — see §5.7. ### 5.4 ISOBUS feature scope | Capability | Value | |---|---| | ISO 11783-10 version | 2 (Second Edition) | -| Generation | 1 (TC-SC) | +| Generation | 1 (TC-SC), plus TRACK (Tramline 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 / 507 / 508 / 509 / 510 / 511 / 514 / 515 (Tramline/TRACK, Level 1 — see §5.7); plus speed/distance/guidance DDIs from the tractor side | ### 5.5 Virtual Terminal UI @@ -280,6 +317,65 @@ 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 | 8 | Time/date (PGN 65254) | `timeDateActive` — set whenever the TC's `TimeDateInterface` is actively broadcasting FEE6 (i.e. no duplicate Time/Date provider has been detected on the bus). Cleared if another ECU's FEE6 is seen. | +| 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. +- 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, TC is the sole Time/Date provider on the bus): `[0x06, 0x00, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00]`. Byte 3 drops to `0x78` (Time/date bit cleared) if another ECU's FEE6 is detected and the TC suppresses its own broadcast. + +**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 Tramline / TRACK Control + +The TC implements AEF/ISO 11783 Task Controller TRACK (Tramline Control), **Level 1 only**: it reports track/guidance-line info to the implement. It does not compute tramline valve states itself (Level 3) or handle the extended Level 2 setup DDIs beyond negotiation. + +**Capability announcement**: at TC control-function claim, PGN 64654 (Control Function Functionalities, source = the TC's own address — see §5.2) advertises `TaskControllerBasicServer` v1, `TaskControllerSectionControlServer` v1 (1 boom / 64 sections), and functionality 27 (TC-TRAM) v1, telling the implement this TC supports tramline. + +**Negotiation (DDI 505/506 handshake)**: when a client's DDOP is registered, the TC scans it for DDI 505 (`TramlineControlLevel`), 506 (`SetpointTramlineControlLevel`), 515 (`TramlineControlState`), and the track DDIs (508-511). + +1. The implement reports DDI 505 as a **bitmask** (bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3). +2. If the implement also has DDI 506, the TC immediately writes DDI 506 back as an **enum** (`0`=no common level, `1`=Level 1, `2`=Level 2, `3`=Level 3) — currently always `1`, since only Level 1 is implemented, even if the implement also advertises Level 2/3. +3. The implement's DDI 506 echo marks negotiation complete for that client. Only then does the TC start sending live track data to it. + +**Live data** (every 250 ms per negotiated client, `MyTCServer::send_tramline_track_data()`): + +| DDI | Name | Sent when | +|---|---|---| +| 507 | TramlineSequenceNumber | Track-valid only. Increments whenever `ActualTrackNumber` or the reference line ID changes — not on section-control toggles. | +| 508 | UniqueABGuidanceReferenceLineID | Track-valid only. | +| 509 | ActualTrackNumber | Track-valid only. 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 | TrackNumberToTheRight / TrackNumberToTheLeft | Track-valid only. | +| 0x0200 / 0x0201 | GuidanceLineSwathWidth / GuidanceLineDeviation | Track-valid only. Swath is currently hardcoded to 6000 mm (TODO: derive from DDOP geometry); deviation comes from AOG's XTE (PGN 0xF2, see §2.5). | +| 514 | GNSSQuality | Always, independent of track validity — it's a positioning signal, not a track signal. Sourced from AOG PGN `0xD6` (see §2.5). | +| 515 | TramlineControlState | **Not** sent from here. Owned solely by section-control-mode handling (PGN `0xF1`, see §2.5) — do not write it elsewhere. | + +"Track-valid" means the TC has an accepted PGN `0xF4` payload with the valid flag set and a non-zero reference line ID (0 = AOG's "no active track" convention). Since AOG only sends `0xF4` on change — there's no heartbeat — track validity is *not* cleared just because no new `0xF4` has arrived; it's only cleared by an explicit "guidance off" packet from AOG, or by AOG disconnecting entirely (no packets of any kind for 3 s). + --- ## 6. Example clients diff --git a/include/app.hpp b/include/app.hpp index d74de11..5c529dd 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -12,19 +12,24 @@ #include #include #include +#include #include #include #include "isobus/hardware_integration/can_hardware_plugin.hpp" #include "isobus/isobus/isobus_functionalities.hpp" #include "isobus/isobus/isobus_speed_distance_messages.hpp" +#include "isobus/isobus/isobus_time_date_interface.hpp" #include "isobus/isobus/isobus_virtual_terminal_client.hpp" #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" +#include "tractor_facilities.hpp" #include "udp_connections.hpp" class Application @@ -76,6 +81,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); @@ -86,6 +93,19 @@ class Application std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; + std::unique_ptr tractorFacilities; + std::unique_ptr timeDateInterface; + std::uint32_t lastFee6TransmitMs = 0; ///< Timestamp of last FEE6 transmission + std::uint32_t lastExternalFee6Ms = 0; ///< Timestamp of last FEE6 from another ECU (0 = never) + bool fee6Broadcasting = false; ///< Whether we are actively broadcasting FEE6 + /// Guards lastExternalFee6Ms/fee6Broadcasting: TimeDateInterface's event listener + /// (registered in setup_tecu_interfaces()) fires from the isobus stack's background + /// thread — not deferred like MyTCServer's callbacks — concurrently with + /// Application::update()'s FEE6 broadcast logic on the main thread. See + /// docs/CONCURRENCY.md. + std::mutex fee6Mutex; + static constexpr std::uint32_t FEE6_TX_INTERVAL_MS = 10000; ///< FEE6 broadcast interval (10 s) + static constexpr std::uint32_t FEE6_PROVIDER_TIMEOUT_MS = 30000; ///< If no FEE6 from other ECU for 30 s, assume no provider std::unique_ptr tecuFunctionalities; std::unique_ptr tcFunctionalities; std::shared_ptr vtClient; @@ -103,6 +123,26 @@ class Application std::int32_t lastXteValue = 0; std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; + static constexpr std::uint32_t AOG_CONNECTION_TIMEOUT_MS = 3000; ///< No AOG packet for this long = disconnected + std::uint8_t gnssFixQuality = 0; ///< AOG fix quality: 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float + 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 + + // Guidance track context — real data from AOG PGN 0xF4. + GuidanceTrackProvider trackProvider; + GuidanceTrackContext currentTrackContext; + bool aogWasConnectedForTrack = false; ///< Edge-detection for AOG connect/disconnect transitions + bool trackControlEnabled = false; ///< Track control enabled (separate from section control) + + // Field identity — from AOG PGN 0xF3. Folded into the upper 16 bits of DDI 508 + // (see the PGN 0xF4 handling in update()) 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; + + bool tractorFacilitiesSentOnPowerUp = false; std::uint32_t vtDisconnectedSinceMs = 0; std::uint32_t lastVtStatusUpdateMs = 0; std::uint32_t lastVtSectionUpdateMs = 0; diff --git a/include/field_registry.hpp b/include/field_registry.hpp new file mode 100644 index 0000000..041249f --- /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 A-B 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..0ae7844 --- /dev/null +++ b/include/guidance_track_context.hpp @@ -0,0 +1,165 @@ +/** + * @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 +#include + +#include "logging_utils.hpp" + +/// @brief Decode a little-endian uint16 from a 2-byte span starting at offset. +inline 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. +inline std::int16_t decode_le_i16(std::span data, std::size_t offset) +{ + return static_cast(decode_le_u16(data, offset)); +} + +/** + * @brief Immutable snapshot of guidance-track state consumed by the ISOBUS TRACK sender. + * + * Corresponds to ISOBUS DDIs: + * - guidanceReferenceLineId -> DDI 508 (Unique A-B Guidance Reference Line ID) + * - actualTrackNumber -> DDI 509 (Actual Track Number) + * - trackNumberRight -> DDI 510 (Track Number to the Right) + * - trackNumberLeft -> DDI 511 (Track Number to the Left) + */ +struct GuidanceTrackContext +{ + std::uint32_t guidanceReferenceLineId = 1; ///< DDI 508 — stable synthetic ID for now + 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 + 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): + * 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) + * + * 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; + + /** + * @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) + { + 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; + 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 + << " -> " << outcome << std::endl; + + return ctx; + } + + /// @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() + { + lastSequence_.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 705605e..44b38dd 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" @@ -29,6 +30,16 @@ enum SectionState : std::uint8_t NOT_INSTALLED = 3 ///< Section is not installed }; +// Tramline control level bitmask (ISO 11783-10) +// An implement may support any combination of levels. +enum class TramlineLevel : std::uint8_t +{ + None = 0, + Level1 = 1, ///< Track info: ActualTrackNumber, adjacent tracks, swath width + Level2 = 2, ///< Control state: TramlineControlState, SequenceNumber + Level3 = 4 ///< Direct valve control: SetpointTramlineCondensedWorkState +}; + class ClientState { public: @@ -63,6 +74,47 @@ 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; + // Tramline support — DDI 505 bitmask from implement (raw value, not inferred) + int get_supported_tramline_levels_bitmask() const; + void set_supported_tramline_levels_bitmask(int bitmask); + + // Track negotiation state (DDI 505/506 handshake complete) + bool is_track_negotiation_complete() const; + void set_track_negotiation_complete(bool complete); + + // Track control enabled (separate from section control, Requirement 7) + bool is_track_control_enabled() const; + void set_track_control_enabled(bool enabled); + + void set_actual_tramline_control_state(std::int32_t value); + std::int32_t get_actual_tramline_control_state() const; + std::uint32_t get_tramline_sequence_number() const; + void increment_tramline_sequence_number(); + + // Last sent track number / reference line ID — used to detect changes for DDI 507 sequence increment + std::int32_t get_last_sent_track_number() const; + void set_last_sent_track_number(std::int32_t trackNumber); + std::uint32_t get_last_sent_reference_line_id() const; + void set_last_sent_reference_line_id(std::uint32_t referenceLineId); + + // DDI 505/506 presence flags + bool get_has_tramline_control_level() const; + void set_has_tramline_control_level(bool has); + bool get_has_setpoint_tramline_control_level() const; + void set_has_setpoint_tramline_control_level(bool has); + bool is_setpoint_level_sent() const; + void set_setpoint_level_sent(bool sent); + + // Working width reported live over the bus (DDI 67/68/70). Needed because these DDIs + // are commonly implemented as Device Process Data (DPD) rather than Device Property + // (DPT) — a DPD has no value embedded in the DDOP itself (see isobus's + // DeviceProcessDataObject, which has no value field/getter at all), only a definition. + // The real value only ever arrives as a live process data value message, which is why + // DeviceDescriptorObjectPoolHelper::get_implement_geometry() (a pure static-pool parser) + // can never see it. We store what the client actually reports here instead. + void set_reported_working_width(std::uint16_t elementNumber, isobus::DataDescriptionIndex ddi, std::int32_t widthMm); + bool try_get_reported_working_width(std::uint16_t elementNumber, std::int32_t &widthMm) const; + 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 @@ -77,8 +129,30 @@ class ClientState bool actualWorkState = false; ///< The overall work state actual std::map elementWorkStates; ///< Work state per element (element number -> is working) bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting + int supportedTramlineLevelsBitmask = 0; ///< Raw DDI 505 bitmask from implement (bit 0=L1, bit 1=L2, bit 2=L3) + bool trackNegotiationComplete = false; ///< DDI 505/506 handshake completed + bool trackControlEnabled = false; ///< Track control enabled (separate from section control) + std::int32_t actualTramlineControlState = 0; ///< Actual tramline control state reported by implement (DDI 0x0203) + std::int32_t lastSentTrackNumber = 0; ///< Last track number sent, for DDI 507 change detection + std::uint32_t lastSentReferenceLineId = 0; ///< Last reference line ID sent, for DDI 507 change detection + std::uint32_t tramlineSequenceNumber = 0; ///< Per-client tramline sequence number (DDI 507) + bool hasTramlineControlLevelDDI = false; ///< Implement has DDI 505 (TramlineControlLevel) + bool hasSetpointTramlineControlLevelDDI = false; ///< Implement has DDI 506 (SetpointTramlineControlLevel) + bool setpointLevelSent = false; ///< Whether we've already written DDI 506 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 + + /// @brief Live working width reported by the client for one device element (DDI 67/68/70). + struct ReportedWidth + { + bool hasActual = false; + std::int32_t actual = 0; + bool hasMaximum = false; + std::int32_t maximum = 0; + bool hasDefault = false; + std::int32_t defaultValue = 0; + }; + std::map elementReportedWidthMm; ///< element number -> reported width(s), same priority scheme as DeviceDescriptorObjectPoolHelper (Actual > Maximum > Default) }; // Create the task controller server object, this will handle all the ISOBUS communication for us @@ -115,6 +189,8 @@ class MyTCServer : public isobus::TaskControllerServer void request_measurement_commands(); void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + void update_track_control_enabled(bool enabled); + void send_tramline_track_data(const GuidanceTrackContext &ctx, std::int32_t swathWidthMm, std::int32_t lineDeviationMm, std::uint8_t gnssFixQuality); private: void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); @@ -131,15 +207,16 @@ class MyTCServer : public isobus::TaskControllerServer /// which is what actually invokes every TaskControllerServer override in this /// class (activate_object_pool, on_value_command, ...) — NOT the thread that /// runs Application::update(). Meanwhile Application::update() (and the methods - /// it calls: request_measurement_commands, update_section_states/_control_enabled, - /// get_clients) reads/writes the SAME maps from the main thread. Without this - /// lock, that's a concurrent std::map read + insert/erase — undefined behavior, - /// seen in practice as the TC silently crashing (no exception, no log line) - /// whenever a new control function appeared on the bus. See docs/CONCURRENCY.md - /// before touching clients/uploadedPools or adding a new callback here: every - /// entry point the isobus stack can call into must take this lock - /// (std::recursive_mutex — some of these methods call each other, e.g. - /// update_section_states -> send_section_setpoint_states), and get_clients() - /// must keep returning a copy, not a reference (see its declaration above). + /// it calls: send_tramline_track_data, request_measurement_commands, + /// update_section_states/_control_enabled, get_clients) reads/writes the SAME + /// maps from the main thread. Without this lock, that's a concurrent + /// std::map read + insert/erase — undefined behavior, seen in practice as the + /// TC silently crashing (no exception, no log line) whenever a new control + /// function appeared on the bus. See docs/CONCURRENCY.md before touching + /// clients/uploadedPools or adding a new callback here: every entry point the + /// isobus stack can call into must take this lock (std::recursive_mutex — some + /// of these methods call each other, e.g. 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; }; 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 3e9877e..58b98c5 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,12 +22,15 @@ #include "isobus/utility/system_timing.hpp" #include "task_controller.hpp" +#include "tractor_facilities.hpp" #include "AOG_TC.iop.h" #include "logging_utils.hpp" #include +#include +#include #include #include #include @@ -44,6 +48,101 @@ static std::string format_hex_address(std::uint8_t address) return value.str(); } +// Helper: populate TimeDateInterface::TimeAndDate from the system clock. +// Used as the callback for TimeDateInterface to provide wall-clock time +// for PGN 65254 (FEE6) broadcasts. +static bool get_system_time(isobus::TimeDateInterface::TimeAndDate &td) +{ + auto now = std::chrono::system_clock::now(); + auto time_t_now = std::chrono::system_clock::to_time_t(now); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()) + .count() % + 1000; + + std::tm tm_utc{}; + std::tm tm_local{}; +#if defined(_WIN32) + gmtime_s(&tm_utc, &time_t_now); + localtime_s(&tm_local, &time_t_now); +#else + gmtime_r(&time_t_now, &tm_utc); + localtime_r(&time_t_now, &tm_local); +#endif + + // PGN 65254 (FEE6) requires the main fields to be UTC; localHourOffset/localMinuteOffset + // are what a receiver adds to UTC to reconstruct local time. Derive the real, DST-aware + // offset using only standard functions (avoid non-portable timegm/_mkgmtime). + const std::time_t localSeconds = std::mktime(&tm_local); + const std::time_t utcAsLocalSeconds = std::mktime(&tm_utc); + const long offsetSeconds = static_cast(localSeconds - utcAsLocalSeconds); + + td.year = static_cast(tm_utc.tm_year + 1900); + td.month = static_cast(tm_utc.tm_mon + 1); + td.day = static_cast(tm_utc.tm_mday); + td.hours = static_cast(tm_utc.tm_hour); + td.minutes = static_cast(tm_utc.tm_min); + td.seconds = static_cast(tm_utc.tm_sec); + td.milliseconds = static_cast((ms / 250) * 250); // J1939: 0.25s resolution + td.quarterDays = static_cast(tm_utc.tm_hour / 6); + td.localHourOffset = static_cast(offsetSeconds / 3600); + td.localMinuteOffset = static_cast((offsetSeconds % 3600) / 60); + return true; +} + +// 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) { @@ -398,7 +497,24 @@ 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 Task Controller Tramline (TRACK) Server — Functionality 27 + // This tells the implement we support tramline control. + // The AgIsoStack library doesn't have this enum value yet, so cast it directly. + tcFunctionalities->set_functionality_is_supported( + static_cast(27), + 1, // Version 1 + true); + + log("Init") << "TC announced TC-BAS, TC-SC (1 boom / 64 sections), and TC-TRAM (TRACK) via PGN 64654" << std::endl; + + // Register repetition-rate diagnostic on the TC's PGN request protocol + auto tcPgnReq = tcCF->get_pgn_request_protocol().lock(); + if (tcPgnReq) + { + tcPgnReq->register_request_for_repetition_rate_callback( + 0xFFFF /* Any PGN */, &log_repetition_rate_request, nullptr); + } } void Application::setup_tecu_interfaces() @@ -431,6 +547,57 @@ 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(); + + // Initialize TimeDateInterface for PGN 65254 (FEE6) broadcasting. + // We broadcast FEE6 proactively so implements can discover us as a + // time source without needing to send a REQRR. If another ECU is + // already providing FEE6, we stay silent (duplicate provider detection). + timeDateInterface = std::make_unique(tecuCF, get_system_time); + timeDateInterface->initialize(); + + // Listen for FEE6 from other ECUs to detect duplicate providers. + // If we see FEE6 from another ECU, we suppress our own broadcast. + timeDateInterface->get_event_dispatcher().add_listener( + [this](const isobus::TimeDateInterface::TimeAndDateInformation &info) { + if (info.controlFunction && tecuCF && + info.controlFunction->get_address() != tecuCF->get_address()) + { + // Fires on the isobus stack's background thread — see fee6Mutex's comment. + std::lock_guard lock(fee6Mutex); + if (lastExternalFee6Ms == 0) + { + log("TECU") << "FEE6 provider detected at SA " + << static_cast(info.controlFunction->get_address()) + << " — suppressing our FEE6 broadcast" << std::endl; + if (fee6Broadcasting && tractorFacilities) + { + tractorFacilities->set_time_date_active(false); + fee6Broadcasting = false; + } + } + lastExternalFee6Ms = isobus::SystemTiming::get_timestamp_ms(); + } + }); + log("Init") << "Time/Date interface (PGN 65254 / FEE6) created, interval=" + << FEE6_TX_INTERVAL_MS << " ms" << std::endl; + + // 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( + 0xFFFF /* Any PGN */, &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 { @@ -451,6 +618,38 @@ 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; + 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: 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6+=Estimated/Manual/Simulated. + // 6+ are not valid ISOBUS quality levels but still represent an active (non-GPS) position + // source — e.g. AOG's Simulator mode — so map them to the weakest real fix (1=GNSS) + // rather than 0=No GPS, which would falsely claim there is no position at all. + gnssFixQuality = (quality <= 5) ? quality : 1; + lastGnssQualityMs = isobus::SystemTiming::get_timestamp_ms(); + + if (quality != lastLoggedQuality) + { + std::cout << "[" << get_timestamp() << "] [TRACK][gnss] fix quality byte=" << static_cast(quality) + << " -> DDI514=" << static_cast(gnssFixQuality) << std::endl; + lastLoggedQuality = quality; + } + return; + } + if (src != 0x7F) { return; @@ -475,6 +674,88 @@ 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); + tcServer->update_track_control_enabled(sectionControlState == 1); + } + else if (pgn == 0xEF) // 239 - Machine Data + { + // Wire layout (payload bytes, for future reference — none of this is parsed today): + // [0]=uturn speed [1]=hydLift [2]=geoStop [3]=TRAM (bit0=left marker, bit1=right marker) + // [4..]=section states, SC1-8 then SC9-16 (condensed bitfields) + // Guidance/tramline state now comes exclusively from PGN 0xF4 (see GuidanceTrackProvider); + // the tram bits here were only used by the removed synthetic fallback provider. + 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 (AOG real guidance data) + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // Parse real guidance track context from AOG PGN 0xF4. + // 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 { @@ -573,6 +854,65 @@ bool Application::update() speedMessagesInterface->update(); if (nmea2000MessageInterface) nmea2000MessageInterface->update(); + + // Periodic FEE6 (Time/Date, PGN 65254) broadcast. + // Only transmit if no other FEE6 provider is active on the bus. + if (timeDateInterface && tecuCF && tecuCF->get_address_valid()) + { + std::lock_guard fee6Lock(fee6Mutex); + const bool otherProviderActive = + (lastExternalFee6Ms != 0) && + !isobus::SystemTiming::time_expired_ms(lastExternalFee6Ms, FEE6_PROVIDER_TIMEOUT_MS); + + if (otherProviderActive) + { + // Another ECU is broadcasting FEE6 — stay silent. + if (fee6Broadcasting) + { + std::cout << "[" << get_timestamp() << "] [TECU] Stopping FEE6 broadcast; another provider active" << std::endl; + fee6Broadcasting = false; + if (tractorFacilities) + { + tractorFacilities->set_time_date_active(false); + } + } + } + else + { + // No other provider — broadcast FEE6 at our configured interval. + if (!fee6Broadcasting) + { + std::cout << "[" << get_timestamp() << "] [TECU] Starting FEE6 broadcast (no other provider detected)" << std::endl; + fee6Broadcasting = true; + lastFee6TransmitMs = 0; // Force immediate first transmission + if (tractorFacilities) + { + tractorFacilities->set_time_date_active(true); + } + } + + if (isobus::SystemTiming::time_expired_ms(lastFee6TransmitMs, FEE6_TX_INTERVAL_MS)) + { + isobus::TimeDateInterface::TimeAndDate td; + if (get_system_time(td)) + { + if (timeDateInterface->send_time_and_date(td)) + { + lastFee6TransmitMs = isobus::SystemTiming::get_timestamp_ms(); + } + } + } + } + } + + // Transmit PGN 65033 once on power-up (ISO 11783-7 B.24.3 repetition + // rate: "on power-up, and then on request"). + if (!tractorFacilitiesSentOnPowerUp && tractorFacilities) + { + tractorFacilities->send_facilities_response(); + tractorFacilitiesSentOnPowerUp = true; + } + if (vtClient) update_vt_client(); @@ -719,9 +1059,50 @@ bool Application::update() } } + // Send tramline track data to implement every 250 ms + static std::uint32_t lastTramlineSendMs = 0; + if (tcServer && isobus::SystemTiming::time_expired_ms(lastTramlineSendMs, 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; + + // GuidanceLineDeviation (DDI 0x0201) from AOG's XTE + std::int32_t lineDevMm = lastXteValue; + + // GuidanceLineSwathWidth (DDI 0x0200) — 6000mm for ESPRO; TODO: derive from DDOP geometry + std::int32_t swathMm = 6000; + + // PGN 0xD6 (GPS fix quality) is a separate, independently-timed stream from 0xF4 — + // treat it as unknown if it goes stale on its own, even while AOG overall is connected. + // 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 zero position + // fix and can get commands rejected, exactly the failure the old hardcoded-4 hack + // was working around. + const bool gnssQualityFresh = (lastGnssQualityMs != 0) && + !isobus::SystemTiming::time_expired_ms(lastGnssQualityMs, GNSS_QUALITY_TIMEOUT_MS); + const std::uint8_t effectiveGnssQuality = gnssQualityFresh ? gnssFixQuality : 1; + + tcServer->send_tramline_track_data(currentTrackContext, swathMm, lineDevMm, effectiveGnssQuality); + lastTramlineSendMs = 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()) @@ -798,27 +1179,54 @@ Application::ImplementDetails Application::derive_implement_details(ClientState } } + // section.width_mm/subBoom.width_mm only reflect a static Device Property (DPT) value + // baked into the DDOP. Many implements report working width as a Device Process Data + // (DPD) instead — which has no value in the DDOP at all, only a definition — so when the + // static pool doesn't have it, fall back to whatever the client has actually reported + // live over the bus (see ClientState::try_get_reported_working_width and the + // ActualWorkingWidth/MaximumWorkingWidth/DefaultWorkingWidth cases in on_value_command). for (const auto &boom : geometry.booms) { for (const auto §ion : boom.sections) { + std::int32_t widthMm = 0; if (section.width_mm) { - totalWidthMillimetres += section.width_mm.get(); + widthMm = section.width_mm.get(); + } + else + { + state.try_get_reported_working_width(section.elementNumber, widthMm); } + totalWidthMillimetres += widthMm; } for (const auto &subBoom : boom.subBooms) { - if (subBoom.sections.empty() && subBoom.width_mm) + if (subBoom.sections.empty()) { - totalWidthMillimetres += subBoom.width_mm.get(); + std::int32_t widthMm = 0; + if (subBoom.width_mm) + { + widthMm = subBoom.width_mm.get(); + } + else + { + state.try_get_reported_working_width(subBoom.elementNumber, widthMm); + } + totalWidthMillimetres += widthMm; } for (const auto §ion : subBoom.sections) { + std::int32_t widthMm = 0; if (section.width_mm) { - totalWidthMillimetres += section.width_mm.get(); + widthMm = section.width_mm.get(); + } + else + { + state.try_get_reported_working_width(section.elementNumber, widthMm); } + totalWidthMillimetres += widthMm; } } } @@ -1161,7 +1569,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); @@ -1198,15 +1606,23 @@ void Application::update_vt_status_strings(bool aogConnected) std::string workingWidth = "n/a"; std::string boomOffset = "n/a"; std::uint8_t implementSections = 0; - if (!clients.empty()) + int implementTramlineLevels = 0; + + // Find the first client with sections (the implement), skip tractors with 0 sections + for (auto &client : clients) { - auto &state = clients.begin()->second; - const ImplementDetails details = derive_implement_details(state); - implementName = details.displayName; - sectionControl = state.is_section_control_enabled() ? "ENABLED" : "DISABLED"; - implementSections = details.sections; - workingWidth = details.widthText.empty() ? "n/a" : details.widthText; - boomOffset = details.boomOffsetText.empty() ? "n/a" : details.boomOffsetText; + if (client.second.get_number_of_sections() > 0) + { + auto &state = client.second; + const ImplementDetails details = derive_implement_details(state); + implementName = details.displayName; + sectionControl = state.is_section_control_enabled() ? "ENABLED" : "DISABLED"; + implementSections = details.sections; + workingWidth = details.widthText.empty() ? "n/a" : details.widthText; + boomOffset = details.boomOffsetText.empty() ? "n/a" : details.boomOffsetText; + implementTramlineLevels = state.get_supported_tramline_levels_bitmask(); + break; // Use the first implement with sections + } } const std::string implementDisplayName = implementName.substr(0, 16); const std::string activeDDOP = clients.empty() ? "none" : implementDisplayName; @@ -1234,6 +1650,39 @@ void Application::update_vt_status_strings(bool aogConnected) mainImplementStatus << "Name " << implementDisplayName << '\n' << "Sections " << totalSections << '\n' << "Section control " << sectionControl; + + // Live guidance-track state from AOG + capability level from implement + { + std::string tramLive; + if (!aogConnected) + { + tramLive = "n/a"; + } + else if (currentTrackContext.valid) + { + std::ostringstream liveStr; + liveStr << "ref:" << currentTrackContext.guidanceReferenceLineId + << " track:" << currentTrackContext.actualTrackNumber; + tramLive = liveStr.str(); + } + else + { + tramLive = "OFF"; + } + + std::string tramCaps; + if (implementTramlineLevels & static_cast(TramlineLevel::Level1)) + tramCaps += "L1 "; + if (implementTramlineLevels & static_cast(TramlineLevel::Level2)) + tramCaps += "L2 "; + if (implementTramlineLevels & static_cast(TramlineLevel::Level3)) + tramCaps += "L3 "; + if (tramCaps.empty()) + tramCaps = "NONE"; + + mainImplementStatus << "\nTram state " << tramLive + << "\nTram levels " << tramCaps; + } send_vt_string_if_changed(VTSectionsFromAOGS, mainImplementStatus.str()); std::ostringstream distanceText; 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/task_controller.cpp b/src/task_controller.cpp index 660da73..62a07f1 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -324,6 +324,155 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool & return false; } +void ClientState::set_reported_working_width(std::uint16_t elementNumber, isobus::DataDescriptionIndex ddi, std::int32_t widthMm) +{ + auto &report = elementReportedWidthMm[elementNumber]; + switch (ddi) + { + case isobus::DataDescriptionIndex::ActualWorkingWidth: + report.hasActual = true; + report.actual = widthMm; + break; + case isobus::DataDescriptionIndex::MaximumWorkingWidth: + report.hasMaximum = true; + report.maximum = widthMm; + break; + case isobus::DataDescriptionIndex::DefaultWorkingWidth: + report.hasDefault = true; + report.defaultValue = widthMm; + break; + default: + break; + } +} + +bool ClientState::try_get_reported_working_width(std::uint16_t elementNumber, std::int32_t &widthMm) const +{ + auto it = elementReportedWidthMm.find(elementNumber); + if (it == elementReportedWidthMm.end()) + { + return false; + } + // Priority order: Actual > Maximum > Default, same as DeviceDescriptorObjectPoolHelper::get_width_with_priority() + const auto &report = it->second; + if (report.hasActual && 0 != report.actual) + { + widthMm = report.actual; + return true; + } + if (report.hasMaximum && 0 != report.maximum) + { + widthMm = report.maximum; + return true; + } + if (report.hasDefault && 0 != report.defaultValue) + { + widthMm = report.defaultValue; + return true; + } + return false; +} + +int ClientState::get_supported_tramline_levels_bitmask() const +{ + return supportedTramlineLevelsBitmask; +} + +void ClientState::set_supported_tramline_levels_bitmask(int bitmask) +{ + supportedTramlineLevelsBitmask = bitmask; +} + +bool ClientState::is_track_negotiation_complete() const +{ + return trackNegotiationComplete; +} + +void ClientState::set_track_negotiation_complete(bool complete) +{ + trackNegotiationComplete = complete; +} + +bool ClientState::is_track_control_enabled() const +{ + return trackControlEnabled; +} + +void ClientState::set_track_control_enabled(bool enabled) +{ + trackControlEnabled = enabled; +} + +void ClientState::set_actual_tramline_control_state(std::int32_t value) +{ + actualTramlineControlState = value; +} + +std::int32_t ClientState::get_actual_tramline_control_state() const +{ + return actualTramlineControlState; +} + +std::uint32_t ClientState::get_tramline_sequence_number() const +{ + return tramlineSequenceNumber; +} + +void ClientState::increment_tramline_sequence_number() +{ + tramlineSequenceNumber++; +} + +std::int32_t ClientState::get_last_sent_track_number() const +{ + return lastSentTrackNumber; +} + +void ClientState::set_last_sent_track_number(std::int32_t trackNumber) +{ + lastSentTrackNumber = trackNumber; +} + +std::uint32_t ClientState::get_last_sent_reference_line_id() const +{ + return lastSentReferenceLineId; +} + +void ClientState::set_last_sent_reference_line_id(std::uint32_t referenceLineId) +{ + lastSentReferenceLineId = referenceLineId; +} + +bool ClientState::get_has_tramline_control_level() const +{ + return hasTramlineControlLevelDDI; +} + +void ClientState::set_has_tramline_control_level(bool has) +{ + hasTramlineControlLevelDDI = has; +} + +bool ClientState::get_has_setpoint_tramline_control_level() const +{ + return hasSetpointTramlineControlLevelDDI; +} + +void ClientState::set_has_setpoint_tramline_control_level(bool has) +{ + hasSetpointTramlineControlLevelDDI = has; +} + +bool ClientState::is_setpoint_level_sent() const +{ + return setpointLevelSent; +} + +void ClientState::set_setpoint_level_sent(bool sent) +{ + setpointLevelSent = sent; +} + MyTCServer::MyTCServer(std::shared_ptr internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -536,6 +685,60 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p log("TC Server") << "WARNING: No supported section control method detected! " << "Device has no DDI 290, 161 (settable), or 141 (settable)." << std::endl; } + + // === Tramline capability detection === + // Scan DDOP for tramline-related DDIs to build element number mappings. + // NOTE: We do NOT infer supported levels from DDI presence here. + // The actual supported-level bitmask comes from DDI 505 (TramlineControlLevel) + // which the implement reports after activation. See on_value_command() DDI 505 handler. + bool hasTramlineControlLevel = false; + bool hasSetpointTramlineControlLevel = false; + bool hasTramlineControlState = false; + bool hasTrackDDIs = false; + + for (std::uint16_t i = 0; i < state.get_pool().size(); i++) + { + auto obj = state.get_pool().get_object_by_index(i); + if (!obj || obj->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceProcessData) + continue; + + auto pd = std::dynamic_pointer_cast(obj); + auto ddi = pd->get_ddi(); + + if (ddi == static_cast(isobus::DataDescriptionIndex::TramlineControlLevel)) + hasTramlineControlLevel = true; + else if (ddi == static_cast(isobus::DataDescriptionIndex::SetpointTramlineControlLevel)) + hasSetpointTramlineControlLevel = true; + else if (ddi == static_cast(isobus::DataDescriptionIndex::TramlineControlState)) + hasTramlineControlState = true; + else if (ddi == static_cast(isobus::DataDescriptionIndex::ActualTrackNumber) || + ddi == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheRight) || + ddi == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheLeft) || + ddi == static_cast(isobus::DataDescriptionIndex::UniqueABGuidanceReferenceLineID)) + hasTrackDDIs = true; + } + + state.set_has_tramline_control_level(hasTramlineControlLevel); + state.set_has_setpoint_tramline_control_level(hasSetpointTramlineControlLevel); + + // Supported levels bitmask starts at 0 — real value comes from DDI 505 callback. + // Track negotiation also starts incomplete; it completes after DDI 505/506 handshake. + state.set_supported_tramline_levels_bitmask(0); + state.set_track_negotiation_complete(false); + + if (hasTramlineControlLevel || hasTrackDDIs || hasTramlineControlState) + { + std::cout << "[" << get_timestamp() << "] [TC] Implement has tramline DDIs in DDOP" + << " (505=" << (hasTramlineControlLevel ? "yes" : "no") + << " 506=" << (hasSetpointTramlineControlLevel ? "yes" : "no") + << " 515=" << (hasTramlineControlState ? "yes" : "no") + << " tracks=" << (hasTrackDDIs ? "yes" : "no") + << ") — waiting for DDI 505 to determine supported levels" << std::endl; + } + else + { + std::cout << "[" << get_timestamp() << "] [TC] Implement has no tramline DDIs in DDOP" << std::endl; + } } else { @@ -687,6 +890,104 @@ bool MyTCServer::on_value_command(std::shared_ptr partn } } } + break; + + // Working width DDIs — often implemented as Device Process Data (no static value in + // the DDOP; see the comment on ClientState::set_reported_working_width), so the only + // way to learn the real width is to capture it here once the client reports it. + case static_cast(isobus::DataDescriptionIndex::ActualWorkingWidth): + case static_cast(isobus::DataDescriptionIndex::MaximumWorkingWidth): + case static_cast(isobus::DataDescriptionIndex::DefaultWorkingWidth): + { + clients[partner].set_reported_working_width(elementNumber, static_cast(dataDescriptionIndex), processDataValue); + std::cout << "[" << get_timestamp() << "] [TC] Element " << elementNumber << " reported working width DDI " + << dataDescriptionIndex << "=" << processDataValue << " mm" << std::endl; + } + break; + + // Tramline DDIs — store actual values reported by implement + case static_cast(isobus::DataDescriptionIndex::TramlineControlLevel): + { + // DDI 505: Implement reports its supported tramline levels as a BITMASK. + // Bit 0 = Level 1 (track info), Bit 1 = Level 2 (extended setup), Bit 2 = Level 3 (TC calculates) + // Example: value=3 means Level 1 + Level 2 supported (NOT "Level 3"). + // Handshake: respond by writing DDI 506 (SetpointTramlineControlLevel) = what we want to use. + // IMPORTANT: DDI 505 is a BITMASK, but DDI 506 is an ENUM: + // 0 = No common Level, 1 = Level 1, 2 = Level 2, 3 = Level 3 + std::cout << "[" << get_timestamp() << "] [TC] Implement reports TramlineControlLevel=" << processDataValue + << " (bitmask: L1=" << ((processDataValue & 0x01) ? "yes" : "no") + << " L2=" << ((processDataValue & 0x02) ? "yes" : "no") + << " L3=" << ((processDataValue & 0x04) ? "yes" : "no") << ")" << std::endl; + clients[partner].set_element_number_for_ddi( + static_cast(dataDescriptionIndex), elementNumber); + clients[partner].set_supported_tramline_levels_bitmask(processDataValue); + + // If implement has DDI 506 and we haven't sent the setpoint yet, do it now. + // We only fully implement Level 1 behavior, so always request Level 1 (enum value 1) + // even if the implement also advertises Level 2 or Level 3. + if (clients[partner].get_has_setpoint_tramline_control_level() && + !clients[partner].is_setpoint_level_sent() && + clients[partner].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTramlineControlLevel)) + { + std::int32_t requestedLevel = 0; // No common level if implement doesn't support L1 + if (processDataValue & 0x01) // Bit 0 = Level 1 support + requestedLevel = 1; // Request Level 1 (enum) + + send_set_value(partner, + static_cast(isobus::DataDescriptionIndex::SetpointTramlineControlLevel), + clients[partner].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTramlineControlLevel), + requestedLevel); + clients[partner].set_setpoint_level_sent(true); + std::cout << "[" << get_timestamp() << "] [TC] Wrote SetpointTramlineControlLevel=" << requestedLevel + << " (Level " << requestedLevel << ")" << std::endl; + } + } + break; + + case static_cast(isobus::DataDescriptionIndex::SetpointTramlineControlLevel): + { + // DDI 506 echo — implement confirms the level we requested. + // When we receive this echo, the DDI 505/506 negotiation is complete + // and we can begin normal TRACK data transmission. + std::cout << "[" << get_timestamp() << "] [TC] SetpointTramlineControlLevel echo=" << processDataValue + << ((processDataValue == 1) ? " — negotiation complete" : " — no common level") << std::endl; + clients[partner].set_element_number_for_ddi( + static_cast(dataDescriptionIndex), elementNumber); + clients[partner].set_track_negotiation_complete(processDataValue == 1); + } + break; + + case static_cast(isobus::DataDescriptionIndex::ActualTrackNumber): + { + // DDI 509 echo from implement — store element mapping only. + // Track data flows through GuidanceTrackContext, not per-client storage. + clients[partner].set_element_number_for_ddi( + static_cast(dataDescriptionIndex), elementNumber); + std::cout << "[" << get_timestamp() << "] [TC] ActualTrackNumber echo=" << processDataValue << std::endl; + } + break; + + case static_cast(isobus::DataDescriptionIndex::TramlineControlState): + { + clients[partner].set_actual_tramline_control_state(processDataValue); + clients[partner].set_element_number_for_ddi( + static_cast(dataDescriptionIndex), elementNumber); + std::cout << "[" << get_timestamp() << "] [TC] TramlineControlState=" << processDataValue << std::endl; + } + break; + + default: + // Handle ActualTramlineCondensedWorkState DDIs (Level 3 feedback from implement) + if ((dataDescriptionIndex >= static_cast(isobus::DataDescriptionIndex::ActualTramlineCondensedWorkState1_16) && + dataDescriptionIndex <= static_cast(isobus::DataDescriptionIndex::ActualTramlineCondensedWorkState209_224)) || + dataDescriptionIndex == static_cast(isobus::DataDescriptionIndex::TramlineSequenceNumber) || + dataDescriptionIndex == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheRight) || + dataDescriptionIndex == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheLeft)) + { + clients[partner].set_element_number_for_ddi( + static_cast(dataDescriptionIndex), elementNumber); + } + break; } return true; @@ -812,6 +1113,137 @@ void MyTCServer::request_measurement_commands() } } + // Subscribe to tramline DDIs (OnChange) + 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); + auto ddi = processDataObject->get_ddi(); + + bool isTramlineDDI = + (ddi == static_cast(isobus::DataDescriptionIndex::ActualTrackNumber)) || + (ddi == static_cast(isobus::DataDescriptionIndex::TramlineControlLevel)) || + (ddi == static_cast(isobus::DataDescriptionIndex::SetpointTramlineControlLevel)) || + (ddi == static_cast(isobus::DataDescriptionIndex::TramlineControlState)) || + (ddi == static_cast(isobus::DataDescriptionIndex::TramlineSequenceNumber)) || + (ddi == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheRight)) || + (ddi == static_cast(isobus::DataDescriptionIndex::TrackNumberToTheLeft)) || + (ddi == static_cast(isobus::DataDescriptionIndex::UniqueABGuidanceReferenceLineID)) || + (ddi == static_cast(isobus::DataDescriptionIndex::GuidanceLineSwathWidth)) || + (ddi == static_cast(isobus::DataDescriptionIndex::GuidanceLineDeviation)) || + (ddi == static_cast(isobus::DataDescriptionIndex::GNSSQuality)) || + (ddi >= static_cast(isobus::DataDescriptionIndex::ActualTramlineCondensedWorkState1_16) && + ddi <= static_cast(isobus::DataDescriptionIndex::ActualTramlineCondensedWorkState209_224)); + + if (!isTramlineDDI) + continue; + + // Skip if we already subscribed/mapped this DDI (deduplicate) + if (client.second.has_element_number_for_ddi(static_cast(ddi))) + continue; + + // Find the first parent DeviceElement and subscribe + for (std::uint32_t j = 0; j < client.second.get_pool().size(); j++) + { + auto parentObject = client.second.get_pool().get_object_by_index(j); + if (!parentObject || parentObject->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceElement) + continue; + + auto elementObject = std::dynamic_pointer_cast(parentObject); + bool found = false; + for (std::uint16_t childId : elementObject->get_child_object_ids()) + { + if (childId == processDataObject->get_object_id()) + { + client.second.set_element_number_for_ddi( + static_cast(ddi), elementObject->get_element_number()); + const auto &entry = isobus::DataDictionary::get_entry(ddi); + + if (processDataObject->has_trigger_method( + isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) + { + send_change_threshold_measurement_command( + client.first, ddi, elementObject->get_element_number(), 1); + std::cout << "Subscribed (OnChange) to tramline DDI " << ddi + << " (" << entry.to_string() << ") for element " + << elementObject->get_element_number() << std::endl; + } + else + { + std::cout << "Mapped tramline DDI " << ddi + << " (" << entry.to_string() << ") to element " + << elementObject->get_element_number() << std::endl; + } + found = true; + break; // First parent match only + } + } + if (found) + break; + } + } + + // Subscribe to working width DDIs (OnChange). These are commonly Device Process + // Data rather than a static Device Property, so the DDOP itself never carries a + // value — the client only reports one once we ask for it. Unlike the tramline DDIs + // above, width DDIs can legitimately repeat across several elements (one per + // section/sub-boom), so every matching element is subscribed — no single-DDI dedup. + 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); + auto ddi = processDataObject->get_ddi(); + + bool isWidthDDI = + (ddi == static_cast(isobus::DataDescriptionIndex::ActualWorkingWidth)) || + (ddi == static_cast(isobus::DataDescriptionIndex::MaximumWorkingWidth)) || + (ddi == static_cast(isobus::DataDescriptionIndex::DefaultWorkingWidth)); + + if (!isWidthDDI) + continue; + + for (std::uint32_t j = 0; j < client.second.get_pool().size(); j++) + { + auto parentObject = client.second.get_pool().get_object_by_index(j); + if (!parentObject || parentObject->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceElement) + continue; + + auto elementObject = std::dynamic_pointer_cast(parentObject); + for (std::uint16_t childId : elementObject->get_child_object_ids()) + { + if (childId != processDataObject->get_object_id()) + continue; + + const auto &entry = isobus::DataDictionary::get_entry(ddi); + if (processDataObject->has_trigger_method( + isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) + { + send_change_threshold_measurement_command( + client.first, ddi, elementObject->get_element_number(), 1); + std::cout << "Subscribed (OnChange) to width DDI " << ddi + << " (" << entry.to_string() << ") for element " + << elementObject->get_element_number() << std::endl; + } + else + { + // No OnChange support — fall back to a time interval so we still + // eventually learn the width instead of never subscribing at all. + send_time_interval_measurement_command(client.first, ddi, elementObject->get_element_number(), 1000); + std::cout << "Subscribed (TimeInterval) to width DDI " << ddi + << " (" << entry.to_string() << ") for element " + << elementObject->get_element_number() << std::endl; + } + break; // First parent match for this object is the only one — object IDs are unique + } + } + } + std::cout << "[" << get_timestamp() << "] Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); } @@ -866,6 +1298,9 @@ void MyTCServer::update_section_states(std::vector §ionStates) void MyTCServer::update_section_control_enabled(bool enabled) { + // Section control only — DDI 160 (SectionControlState). + // Track control (DDI 515) is handled separately by update_track_control_enabled() + // even though both are currently triggered by the same AOG Auto command. std::lock_guard lock(clientsMutex); for (auto &client : clients) { @@ -884,6 +1319,36 @@ void MyTCServer::update_section_control_enabled(bool enabled) } } +void MyTCServer::update_track_control_enabled(bool enabled) +{ + // Track control — DDI 515 (TramlineControlState). + // Separate from section control (DDI 160) even though both are currently triggered + // by the same AOG Auto command. This allows future decoupling without changing the UI. + std::lock_guard lock(clientsMutex); + for (auto &client : clients) + { + // Update the local flag + if (client.second.is_track_control_enabled() != enabled) + { + client.second.set_track_control_enabled(enabled); + } + + // Only send DDI 515 to clients that have completed DDI 505/506 negotiation + // and have the TramlineControlState DDI in their DDOP. + if (client.second.is_track_negotiation_complete() && + client.second.has_element_number_for_ddi(isobus::DataDescriptionIndex::TramlineControlState)) + { + // DDI 515 values: 0=manual/off, 1=automatic/on + send_set_value(client.first, + static_cast(isobus::DataDescriptionIndex::TramlineControlState), + client.second.get_element_number_for_ddi(isobus::DataDescriptionIndex::TramlineControlState), + enabled ? 1 : 0); + std::cout << "[" << get_timestamp() << "] [TC] TramlineControlState=" << (enabled ? "On" : "Off") + << " (track control)" << std::endl; + } + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex); @@ -991,3 +1456,74 @@ bool MyTCServer::is_ddi_settable(std::shared_ptr client } return false; } + +void MyTCServer::send_tramline_track_data(const GuidanceTrackContext &ctx, std::int32_t swathWidthMm, std::int32_t lineDeviationMm, std::uint8_t gnssFixQuality) +{ + // Send coherent TRACK context to all clients that have completed DDI 505/506 negotiation. + // Do NOT send track data before negotiation is complete (Requirement 9). + std::lock_guard lock(clientsMutex); + for (auto &client : clients) + { + auto &state = client.second; + + // Gate on negotiation completion + if (!state.is_track_negotiation_complete()) + continue; + + int bitmask = state.get_supported_tramline_levels_bitmask(); + if (bitmask == 0) + continue; + + 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); + } + }; + + // Track-specific DDIs (507-511, swath, deviation) only make sense while a track is + // actually active — GNSS quality below is independent and sent regardless of ctx.valid. + if (ctx.valid) + { + // DDI 507 sequence: increment when track context changes (not on section control toggle). + // Detect change by comparing the actual track number AND the reference line ID against + // the last sent values — a line switch can land on the same track index. + bool contextChanged = (ctx.actualTrackNumber != state.get_last_sent_track_number()) || + (ctx.guidanceReferenceLineId != state.get_last_sent_reference_line_id()); + if (contextChanged) + { + state.increment_tramline_sequence_number(); + state.set_last_sent_track_number(ctx.actualTrackNumber); + state.set_last_sent_reference_line_id(ctx.guidanceReferenceLineId); + } + + // Send DDIs in coherent ordering per the TRACK guideline: + // 507 (sequence) -> 508 (ref line ID) -> 509 (actual track) -> 510 (right) -> 511 (left) + if (state.has_element_number_for_ddi(isobus::DataDescriptionIndex::TramlineSequenceNumber)) + { + trySend(isobus::DataDescriptionIndex::TramlineSequenceNumber, + static_cast(state.get_tramline_sequence_number())); + } + trySend(isobus::DataDescriptionIndex::UniqueABGuidanceReferenceLineID, + static_cast(ctx.guidanceReferenceLineId)); + trySend(isobus::DataDescriptionIndex::ActualTrackNumber, ctx.actualTrackNumber); + trySend(isobus::DataDescriptionIndex::TrackNumberToTheRight, ctx.trackNumberRight); + trySend(isobus::DataDescriptionIndex::TrackNumberToTheLeft, ctx.trackNumberLeft); + + // Supplemental Level 1 DDIs + if (swathWidthMm > 0) + { + trySend(isobus::DataDescriptionIndex::GuidanceLineSwathWidth, swathWidthMm); + } + trySend(isobus::DataDescriptionIndex::GuidanceLineDeviation, lineDeviationMm); + } + + // GNSS Quality (DDI 514): sourced from AOG PGN 0xD6 fix quality byte. Independent of + // track validity, so it's sent whenever the client has the element, active track or not. + // 0=No GPS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float + trySend(isobus::DataDescriptionIndex::GNSSQuality, static_cast(gnssFixQuality)); + + // TramlineControlState (DDI 515) is owned solely by update_track_control_enabled(). + // Do NOT write it from here. + } +} diff --git a/src/tractor_facilities.cpp b/src/tractor_facilities.cpp new file mode 100644 index 0000000..1d1a3b3 --- /dev/null +++ b/src/tractor_facilities.cpp @@ -0,0 +1,453 @@ +/** + * @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/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]; + } + + // Printed unconditionally, like the rest of this codebase's diagnostic + // logging — not gated by a log level. We use a simple hex dump to avoid + // pulling in the full Facilities decode for a diagnostic message. + // The line is only emitted when a PGN 65032 message actually arrives. + 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]); + } + std::cout << "[" << get_timestamp() << "] [TractorFacilities] [Debug] PGN 65032 from SA " + << static_cast(sa) << ": required facilities [" << hex.str() << "]" << std::endl; +} 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; +}