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 167ea70..66dc0cd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ if(NOT DEFINED CAN_DRIVER) list(APPEND CAN_DRIVER "WindowsInnoMakerUSB2CAN") list(APPEND CAN_DRIVER "TouCAN") list(APPEND CAN_DRIVER "SYS_TEC") + list(APPEND CAN_DRIVER "WindowsCandleLight") elseif(APPLE) set(CAN_DRIVER "MacCANPCAN") else() @@ -62,7 +63,7 @@ FetchContent_MakeAvailable(Boost) FetchContent_Declare( isobus GIT_REPOSITORY https://github.com/Open-Agriculture/AgIsoStack-plus-plus.git - GIT_TAG 1d081d9a9df61e256015efda3683d15c8fa3d580 + GIT_TAG 02f3fe910de5fdc73e2014394c2e80900490b9cc DOWNLOAD_EXTRACT_TIMESTAMP TRUE) FetchContent_MakeAvailable(isobus) @@ -159,6 +160,22 @@ if(AOG_TC_VALIDATE_IOP) add_test(NAME object_pool_attributes_in_range COMMAND iop_validator "${AOG_TC_IOP_SOURCE}") + # GuidanceTrackProvider (AOG PGN 0xF4 parsing) unit tests. Made a dependency + # of iop_validator so the validation workflow, which builds only that target, + # also builds this test. + add_executable( + test_guidance_track + ${CMAKE_CURRENT_LIST_DIR}/tools/test_guidance_track.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/guidance_track_context.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/async_log.cpp) + target_compile_features(test_guidance_track PRIVATE cxx_std_20) + set_target_properties(test_guidance_track PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_guidance_track + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + target_link_libraries(test_guidance_track PRIVATE Threads::Threads) + add_dependencies(iop_validator test_guidance_track) + add_test(NAME guidance_track_parsing COMMAND test_guidance_track) + # A deliberately broken pool, so the suite asserts more than "today's pool # passes". One case per message, since a shared exit code stays green as long # as any one check still fires. @@ -194,6 +211,16 @@ if(AOG_TC_VALIDATE_IOP) add_dependencies(iop_validator test_async_log) add_test(NAME async_log COMMAND test_async_log) + # AOG UDP datagram parsing; the parser is header-only. + add_executable(test_aog_packet_parser + ${CMAKE_CURRENT_LIST_DIR}/tools/test_aog_packet_parser.cpp) + target_compile_features(test_aog_packet_parser PRIVATE cxx_std_20) + set_target_properties(test_aog_packet_parser PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_aog_packet_parser + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + add_dependencies(iop_validator test_aog_packet_parser) + add_test(NAME aog_packet_parser COMMAND test_aog_packet_parser) + # element_hierarchy (cached element parents for the section state lookup) unit # tests, built with the validation workflow for the same reason. add_executable(test_element_hierarchy @@ -212,6 +239,37 @@ 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) + + # Sharing the bus with other Tractor ECUs (ISO 11783-9 4.4.2.1) + add_executable( + test_tecu_coexistence + ${CMAKE_CURRENT_LIST_DIR}/tools/test_tecu_coexistence.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/tecu_coexistence.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/tractor_facilities.cpp) + target_compile_features(test_tecu_coexistence PRIVATE cxx_std_20) + set_target_properties(test_tecu_coexistence PROPERTIES CXX_EXTENSIONS OFF) + target_include_directories(test_tecu_coexistence + PRIVATE ${CMAKE_CURRENT_LIST_DIR}/include) + target_link_libraries( + test_tecu_coexistence PRIVATE isobus::Isobus isobus::HardwareIntegration + isobus::Utility) + add_dependencies(iop_validator test_tecu_coexistence) + add_test(NAME tecu_coexistence COMMAND test_tecu_coexistence) endif() if(WIN32) diff --git a/docs/CONCURRENCY.md b/docs/CONCURRENCY.md index 8df81de..3e53ed9 100644 --- a/docs/CONCURRENCY.md +++ b/docs/CONCURRENCY.md @@ -38,8 +38,9 @@ registered: `softKeyEventDispatcher`/`buttonEventDispatcher`/`changeNumericValueEventDispatcher`/etc. synchronously). Any raw `add_global_parameter_group_number_callback`/`add_any_control_function_parameter_group_number_callback` - registered directly in `app.cpp` is in this category too, unless proven otherwise the same way: - read the registration call, not the surrounding comments. + registered directly in `app.cpp`/`tractor_facilities.cpp` (PGN-request handlers, diagnostic + loggers) is in this category too, unless proven otherwise the same way: read the registration + call, not the surrounding comments. If you're not sure which category a new callback falls into, trace it the way this file's history did: find where it's registered with `CANNetworkManager`/`ControlFunction` and check whether that @@ -58,6 +59,21 @@ registration point itself defers to an `update()` call, or invokes the listener/ overrides may turn out to already be main-thread-only — the lock is cheap insurance and keeps every entry point consistent regardless of how the library's internals might change. +- **`OtherTecuMonitor`** (`tecu_coexistence.hpp`/`.cpp`) — `on_frame()` (frame-received event) and its + PGN 65033 callback run on the background thread, `refresh()` and the queries on the main thread. The + per-address and per-message flags are atomics; the facility map is under its own mutex. It lives as long + as `Application` because the stack calls global PGN callbacks from a copy of the list, so one can still run + once after `remove_global_parameter_group_number_callback()` returns. +- **Rebuilding the TECU at runtime** (`Application::teardown_tecu()`, when stepping down to a secondary Tractor + ECU) destroys `TractorFacilities`, `SpeedMessagesInterface`, `NMEA2000MessageInterface` and the functionalities + object before `deactivate_control_function()`. The PGN request protocol and the "any control function" + callbacks are invoked under the same locks their removal takes, so they cannot run against a destroyed + object. `TractorFacilities`' PGN 65032 callback may still run once after removal, which is harmless: it only + logs and never touches the object. `TractorFacilities::providedFacilities` is under its own mutex + (set on the main thread, read when answering a PGN 65033 request). +- **`TractorFacilities::timeDateActive`** — atomic: set on the main thread from the TECU plan, read on the + background thread when answering a request for PGN 65033 or PGN 65254. + ## What's *not* protected yet — known gap Calls into `vtClient` (`isobus::VirtualTerminalClient`) happen from both the main thread diff --git a/docs/LINUX_DAEMON.md b/docs/LINUX_DAEMON.md index 469b8d7..94f8ba9 100644 --- a/docs/LINUX_DAEMON.md +++ b/docs/LINUX_DAEMON.md @@ -125,6 +125,7 @@ The TC reads `settings.json` from `$XDG_CONFIG_HOME/AOG-TaskController/` (typica "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } @@ -138,6 +139,7 @@ The TC reads `settings.json` from `$XDG_CONFIG_HOME/AOG-TaskController/` (typica | `aogHeartbeatEnabled` | `bool` | `true` | Whether the TC sends a 100 ms heartbeat UDP packet to AgIO/AgValonia even when no implement is connected. **Set to `false` if pairing with AgOpenGPS pre-v6.8.2 beta 5** — older AOG versions choke on the heartbeat. | | `vtEnabled` | `bool` | `true` | Whether the TC registers its Virtual Terminal UI. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`–`4`); `3` selects Second Edition Draft. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character ISOBUS language code. | | `countryCode` | `string` | `"US"` | Two-character ISOBUS country code. | diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index 27478d4..161cbf3 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -95,8 +95,11 @@ All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`, except `0xD6` ( | `0xC9` (201) | Subnet detection | 5 | `[0xC9, 0xC9, IP0, IP1, IP2]` | | `0xD6` (214) | GPS/IMU data | variable (≥39 used) | Only byte 38 (fix quality) is parsed today; rest of frame is currently unused. Source `0x7C`. | | `0xE5` (229) | Section states (64 sections) | 8 | Bitfield: bit `8·j + i` of byte `j` is section `(8j + i)` ON/OFF | +| `0xEF` (239) | Machine data | variable | Only used as an AOG-liveness signal; payload not parsed. | | `0xF1` (241) | Section control mode | 1 | `[mode]` where `1` = enabled, `0` = disabled | | `0xF2` (242) | Process data | 6 | `[DDI_lo, DDI_hi, val0, val1, val2, val3]` — DDI is little-endian `uint16`; value is little-endian `int32` | +| `0xF3` (243) | Field name | variable | The open field's name as raw UTF-8; empty = field closed. | +| `0xF4` (244) | Guidance track context | 10 or 12 | Real-time AB-line/track guidance state, announced to implements as DDI 507-513 — see §5.4.1. | #### `0xC9` — Subnet detection @@ -136,10 +139,34 @@ Wraps a single ISO 11783 DDI/value pair. The TC currently dispatches on these DD |---|---|---| | `156` | Actual speed (mm/s) | Stored. If TECU enabled, broadcast as Ground/Wheel/Machine-selected speed (PGN 65256) + NMEA2000 SOG. Drives forward/reverse direction. Also produces J1939 PGN 65256 every 100 ms. | | `597` | Total distance (mm) | Stored and displayed on the VT Status page. If TECU is enabled, also populated into Speed Messages distance fields. | -| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. | +| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. Also announced to implements as DDI 513 (GuidanceLineDeviation) — see §5.4.1. | Unknown DDIs are silently ignored (PGN 0xF2 is the generic process-data channel — the TC will gain more DDIs over time). +#### `0xF3` — Field name + +The whole payload is the open field's name as raw UTF-8 — no length prefix, no offset — up to 248 bytes (longer names are truncated). An empty payload means the field was closed, which also invalidates the current track context. + +The TC maps each field name to a persistent 16-bit index, stored one `index,name` line per field in `field_registry.csv` next to `settings.json`. The index is folded into the upper 16 bits of DDI 508 (see `0xF4`), so a track's reference line ID stays unique across fields. + +#### `0xF4` — Guidance track context + +AOG's real-time AB-line/track guidance state. 12-byte payload (a 10-byte payload without the last field is also accepted): + +``` + Byte 0 Sequence counter (0–255, wraps) + Byte 1 Flags: bit0=valid, bit1=heading same way, bit2=curve mode + Bytes 2-3 Guidance Reference Line ID (uint16 LE) — 0 = no active track + Bytes 4-5 Actual Track Number (int16 LE, signed — can be negative and jump by more than 1) + Bytes 6-7 Track Number Left (int16 LE, signed) + Bytes 8-9 Track Number Right (int16 LE, signed) + Bytes 10-11 Swath Width in mm (uint16 LE) — distance between adjacent tracks; 0 = not reported +``` + +AOG sends this **only when the guidance state actually changes** — there is no heartbeat. The TC rejects any packet whose sequence number isn't strictly ahead of the last accepted one (catches duplicates, freezes, and reordered/stale UDP delivery). + +**Track-number offset:** the TC adds `+1` to all three track numbers (current/left/right) before announcing them — confirmed by field testing, not documented anywhere on AOG's side. As sent raw by AOG, a tramline implement's own on-board phase (which pass of N is "on") was consistently one pass out of sync with AOG's own intended on/off state, for both left and right passes; a uniform `+1` (independent of sign) brought them into agreement. See `GuidanceTrackProvider::parse()` (`AOG_TRACK_NUMBER_OFFSET`). + ### 2.6 PGNs outbound (TC → client) All PGNs sent **by the TC to AgIO/AgValonia** use source `0x80`. @@ -209,6 +236,7 @@ The TC reads `settings.json` from a per-user config directory: "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } @@ -217,11 +245,12 @@ The TC reads `settings.json` from a per-user config directory: | Key | Type | Default | Description | |---|---|---|---| | `subnet` | `int[3]` | `[192, 168, 5]` | First three octets of the LAN AgIO/AgValonia lives on. Used for NIC selection and broadcast destination. | -| `tecuEnabled` | `bool` | `true` | If `true`, the TC also impersonates a Tractor ECU on the CAN bus (claims address 240, broadcasts Speed Messages/NMEA2000, announces Class 1 BasicTractorECUServer). Set `false` when the tractor already has a TECU. | -| `nmeaSendEnabled` | `bool` | `true` | Enable cyclic NMEA2000 COG/SOG transmission. Requires `tecuEnabled`; the VT disables this control when no TECU interface exists. | +| `tecuEnabled` | `bool` | `true` | `true`: the TC is the tractor's Tractor ECU, the primary at address 240 (Speed Messages, NMEA2000, Class 1 BasicTractorECUServer), stepping down if the tractor has one after all. `false`: for tractors with their own TECU; the TC's TECU is a secondary that only adds what the tractor's lacks, and everything when there is none. See §5.7. | +| `nmeaSendEnabled` | `bool` | `true` | Enable cyclic NMEA2000 COG/SOG transmission (not while another Tractor ECU sends it, §5.7). The VT disables this control when no TECU interface exists. | | `aogHeartbeatEnabled` | `bool` | `true` | Send `0xF0` heartbeat to AgIO/AgValonia every 100 ms even with no implement. Disable for AOG < v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Register the Virtual Terminal client and display the TC UI when a VT is present. | | `tcVersion` | `integer` | `3` | Task Controller version code (`0`=DIS, `1`=FDIS.1, `2`=First Edition, `3`=Second Edition Draft, `4`=Second Published Edition). | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised through the ISOBUS language interface. | | `countryCode` | `string` | `"US"` | Two-character country code advertised through the ISOBUS language interface. | @@ -239,8 +268,8 @@ AOG-TaskController [options] |---|---|---| | `--help` | — | Print usage and exit. | | `--version` | — | Print git-describe version (with `-dirty` suffix on a dirty tree) and exit. | -| `--can_adapter=` | none | CAN driver. One of: `peak-pcan`, `innomaker-usb2can`, `rusoku-toucan`, `sys-tec-usb2can` (all Windows), `socketcan` (Linux). **Required** — the TC will not start without a driver. | -| `--can_channel=` | — | Driver-specific channel. Numeric (`1`, `2`, ...) for Windows USB adapters; interface name (`can0`, `vcan0`) for SocketCAN. | +| `--can_adapter=` | none | CAN driver. One of: `peak-pcan`, `innomaker-usb2can`, `rusoku-toucan`, `sys-tec-usb2can`, `candlelight` (alias `gs_usb`; candleLight/gs_usb firmware over WinUSB) (all Windows), `socketcan` (Linux). **Required** — the TC will not start without a driver. | +| `--can_channel=` | — | Driver-specific channel. Numeric (`1`, `2`, ...) for Windows USB adapters (for `candlelight`, which adapter when several are plugged in; empty = `1`); interface name (`can0`, `vcan0`) for SocketCAN. | | `--log_level=` | — | One of `debug`, `info`, `warning`, `error`, `critical`. Filters AgIsoStack log output. | | `--log2file` | off | Also write all output to `/logs/AOG-TaskController_YYYY-M-D_H-M.log`. | @@ -257,7 +286,7 @@ Two control functions, both claiming addresses via standard J1939-81 address cla | CF | NAME function | Address | Notes | |---|---|---|---| | Task Controller | `TaskController` (function code 61) | Preferred `247` (ISO 11783-10 MappingComputer). Walks if claimed. | Always present. | -| Tractor ECU | `TractorECU` (function code 132) | Fixed `240` (non-arbitrary-address-capable per ISO 11783-9). | Only present if `tecuEnabled: true`. | +| Tractor ECU | `TractorECU` (function code 134) | `240` as the primary; an arbitrary address (preferred `128`) as a secondary, see §5.7. | Primary with `tecuEnabled: true`, secondary with `false`. | Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufacturer Code `1407`, Identity `20`. Override these in `app.cpp` if you fork. @@ -267,10 +296,16 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac |---|---|---|---| | `0xCB00` (Process Data) | 2 s | TC | ISO 11783-10 B.8.1 Task Controller Status. Status byte bit 1 = task totals active. | | `0x1F903` (NMEA2000 XTE) | 1 Hz | TC | Cross-track error, derived from AOG's guidance-line deviation PGN. | -| `0xFEE8` (PGN 65256 Speed/Direction) | 100 ms | TECU | Ground/Wheel/Machine-selected speed + machine direction, J1939 format. Only when TECU enabled. | +| `0xFEE8` (PGN 65256 Speed/Direction) | 100 ms | TECU | Ground/Wheel/Machine-selected speed + machine direction, J1939 format. Only when TECU enabled, and not while another Tractor ECU sends it (§5.7). | +| `0xFC8E` (Control Function Functionalities) | At claim + periodic | TC | Announces TaskControllerBasicServer (v1), TaskControllerSectionControlServer (v1, 1 boom / 64 sections) and functionality 27, the Task Controller TRACK server (v1). See §5.4.2. | | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). | -| NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. | +| `0xFE48`, `0xFE49`, `0xF022` (PGN 65096, 65097, 61474) | 100 ms | TECU | Wheel-based speed, ground-based speed, machine selected speed (with distance and direction). As a secondary, only those no other Tractor ECU provides (§5.7). | +| `0xFE09` (PGN 65033 Tractor Facilities) | Power-up + on request, and when it changes | TECU | 8-byte facility bitmask advertising which PGNs the TECU actually broadcasts. See §5.6. | +| `0xFEE6` (PGN 65254 Time/Date) | On request only (ISO 11783-7 B.1) | TECU | UTC from the PC clock plus the local offset, including daylight saving time. A secondary TECU leaves requests to a Tractor ECU that provides it. | +| NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. Not while another Tractor ECU sends it (§5.7). | | GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | GPS fix quality from AgIO (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` if `0xD6` has never arrived, and to `0` once it stops arriving (see §2.5). | +| Guidance track data (DDI 507-513, via `0xCB00` Process Data) | 250 ms, while AOG has a valid track | TC | Track number, adjacent tracks, reference line, swath width and line deviation from AOG's PGN `0xF4`/`0xF2`, sent to each client whose DDOP declares them as settable. See §5.4.1. | +| Track control level and state (DDI 506, 515, via `0xCB00` Process Data) | Level once, state on each AOG section-control mode packet | TC | The track control level the TC picked, then the on/off state — only to clients that completed the negotiation. See §5.4.2. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. @@ -279,17 +314,47 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co - **Device Descriptor Object Pool (DDOP)** uploads from clients (stored per client). - **Condensed actual work-state DDIs** (160, 161, 290, plus the extended range 16001–16016 per the standard): mapped into the per-client section model and forwarded to AgIO/AgValonia as PGN `0xF0`. - **Section control state DDI**: tracked per client. +- **Track control DDIs (505, 506, 515)**: negotiated and tracked per client — see §5.4.2. - **Process data acknowledges (PDACK)**: logged. +- **PGN 65033 requests**: answered with the Tractor Facilities response (§5.6). An implement may also send PGN 65032 (Required Tractor Facilities) to advertise what it needs; the TC logs this at debug level but does not change its response. ### 5.4 ISOBUS feature scope | Capability | Value | |---|---| | ISO 11783-10 version | 2 (Second Edition) | -| Generation | 1 (TC-SC) | +| Generation | 1 (TC-SC), plus TRACK (Track Control) Level 1 | | Max booms | 1 | | Max sections | 64 | -| Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states), plus speed/distance/guidance DDIs from the tractor side | +| Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states); 505 / 506 / 515 (track control, see §5.4.2); 507-511 / 513 / 514 (guidance data, see §5.4.1); plus speed/distance/guidance DDIs from the tractor side | + +#### 5.4.1 Guidance data sent to implements + +The TC pushes guidance data to each client whose DDOP declares the DDI as settable. Clients can't request values from the TC, so a DDI a client doesn't declare, or declares as not settable, is never sent to it. Only clients with sections (implements, not tractors) are considered, as with the other DDI mappings. + +| DDI | Name | Sent when | +|---|---|---| +| 507 | GuidanceTrackSequenceNumber | Valid track. Increments whenever the actual track number or the reference line ID changes — not on section-control toggles. | +| 508 | UniqueGuidanceReferenceLineID | Valid track. AOG's 16-bit reference line ID in the low 16 bits, the persistent field index (see `0xF3`) in the high 16 bits. | +| 509 | ActualGuidanceTrackNumber | Valid track. Signed; can be negative and can jump by more than 1 in a single update (e.g. skipping several tracks on a headland turn). | +| 510 / 511 | GuidanceTrackNumberToTheRight / ...ToTheLeft | Valid track. | +| 512 | GuidanceLineSwathWidth | Valid track, and AOG reported a non-zero swath width in `0xF4`. AOG's track spacing (tool width minus overlap), the same spacing the track numbers are derived from. | +| 513 | GuidanceLineDeviation | Valid track. AOG's XTE in mm (PGN `0xF2`, see §2.5). | +| 514 | GNSSQuality | Always — see `0xD6`. | + +"Valid track" means the TC holds an accepted PGN `0xF4` payload with the valid flag set, a non-zero reference line ID, and an open field (PGN `0xF3`) to scope the ID to. Since AOG only sends `0xF4` on change, validity is *not* cleared just because no new `0xF4` has arrived; it is cleared only by an explicit "guidance off" packet, by the field closing, or by AOG disconnecting entirely (no packets of any kind for 3 s). + +#### 5.4.2 Track control level negotiation + +At TC control-function claim, PGN 64654 (Control Function Functionalities, source = the TC's own address — see §5.2) announces functionality 27, the Task Controller TRACK server, to tell implements this TC supports track control. + +For each client whose DDOP declares DDI 505 and 506 the TC negotiates a level: + +1. The implement reports DDI 505 (`SupportedTrackControlLevels`) as a **bitmask** (bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3). +2. The TC writes DDI 506 (`SetpointTrackControlLevel`) back as an **enum** (`0` = no common level, `1` = Level 1, `2` = Level 2, `3` = Level 3). Only Level 1 is implemented, so this is `1` whenever the implement supports Level 1, even if it also advertises Level 2 or 3. +3. The implement's DDI 506 echo completes the negotiation, if it echoes `1`. + +Only DDI 515 (`TrackControlState`: `0` = manual/off, `1` = automatic/on) depends on the negotiation: it is written to clients that completed it, each time AOG's section control mode (PGN `0xF1`, see §2.5) arrives. The guidance data in §5.4.1 is sent whether or not a client negotiated a level. ### 5.5 Virtual Terminal UI @@ -299,6 +364,49 @@ 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) | The TECU sends it: always as the primary; as a secondary only if no other Tractor ECU provides it (§5.7). | +| 1 | 3 | Wheel-based speed (PGN 65096) | Same rule. | +| 3 | 8 | Time/date (PGN 65254) | Our TECU answers requests for it: as the primary always, as a secondary only when no lower Tractor ECU advertises or sends it. | +| 3 | 7,6 | Ground-based distance + direction | Same as ground-based speed. | +| 3 | 5,4 | Wheel-based distance + direction | Same as wheel-based speed. | +| 5 | 3 | Selected speed (machine selected speed, PGN 61474) | Same rule. | + +**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. +- 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 as the primary): `[0x06, 0x00, 0xF8, 0x00, 0x04, 0x00, 0x00, 0x00]`. + +**PGN 65032 (Required Tractor Facilities):** When an implement broadcasts what it needs, the TC logs the request at debug level. The response is not modified based on the implement's requirements — a facility bit is set to 1 only when backed by a live broadcast. + +### 5.7 Sharing the bus with a tractor's Tractor ECU + +ISO 11783-9:2012 §4.4.2.1 allows several Tractor ECUs on the implement bus. The one with function instance 0 is the primary; one with a higher instance "shall not provide any messages which are provided by a Tractor ECU with a lower function instance", requests PGN 65033 from those, and sets only the facility bits they lack (the standard's example is a display adding ground-based speed from GNSS to a tractor without radar). + +- **`tecuEnabled` picks the role.** `true`: AOG is the tractor's TECU. At startup the TC's own address claim makes every control function on the bus announce itself; if no other Tractor ECU (function code 134) is there, our TECU claims `240` as the primary (function instance 0) and sends everything in §5.2, otherwise it starts as a secondary. `false` ("the tractor has its own TECU"): always a secondary at an arbitrary address, with a function instance above every Tractor ECU on the bus. With no other Tractor ECU there, every message is a gap and it sends them all; once the tractor's appears, it cuts back to what that one lacks. +- **Never taking `240` from a tractor:** our TECU NAME is arbitrary-address-capable in both roles. That is the most significant NAME bit, so in an address claim contest for `240` our primary loses to a fixed-address Tractor ECU and moves elsewhere, instead of knocking the tractor's ECU off the bus. +- **Stepping down at runtime** (`tecuEnabled: true`): once a second the TC checks for other Tractor ECUs. If another one with function instance 0 appears while ours is the primary, or ours lost `240`, our TECU is rebuilt as a secondary above every Tractor ECU's function instance. Another secondary appearing is no reason to step down. The operator sees a hardware message ("Tractor ECU found: AOG adds only missing data"). +- **What a secondary sends:** nothing until the other Tractor ECUs have answered PGN 65033 (and at least 1 s has passed) or 2.5 s have passed. Then each of ground-based speed, wheel-based speed, machine selected speed, J1939 vehicle direction/speed and NMEA 2000 COG/SOG is sent only if no Tractor ECU with a lower function instance advertises it in PGN 65033 *and* none has sent it in the last 3 s. Answering requests for time/date follows the same rule with a 30 s window, because another Tractor ECU may only send it when asked. Only lower instances count, so two secondaries never wait on each other. Its PGN 65033 advertises exactly what it sends, and is re-sent when that changes. +- A secondary does not step back up to primary if the tractor's Tractor ECU leaves; it then sends everything, still as a secondary. + --- ## 6. Example clients diff --git a/include/aog_packet_parser.hpp b/include/aog_packet_parser.hpp new file mode 100644 index 0000000..8e779f0 --- /dev/null +++ b/include/aog_packet_parser.hpp @@ -0,0 +1,78 @@ +/** + * @brief Splits one AgIO/AOG UDP datagram into its packets + * + * A packet is: 0x80 0x81, source, PGN, length, data bytes, CRC. + * AgIO never splits a packet across datagrams, so each datagram is parsed on its own + * and nothing is carried over to the next one. + */ + +#pragma once + +#include +#include +#include + +namespace aog_packet +{ + inline constexpr std::uint8_t START_HIGH = 0x80; + inline constexpr std::uint8_t START_LOW = 0x81; + inline constexpr std::size_t HEADER_SIZE = 5; ///< Start (2), source, PGN, length + inline constexpr std::size_t CRC_SIZE = 1; + + /// @brief Why parsing a datagram stopped before its end + enum class ParseStop + { + Complete, ///< Every byte belonged to a packet + UnknownStart, ///< Bytes at the stop offset do not start with 0x80 0x81 + Truncated ///< A packet header claims more bytes than the datagram holds + }; + + struct ParseResult + { + ParseStop stop = ParseStop::Complete; + std::size_t offset = 0; ///< Where parsing stopped; equals the datagram size when complete + std::size_t packets = 0; ///< Number of packets handed to the callback + }; + + /** + * @brief Hands every complete packet in a datagram to a callback, in order + * @param datagram The bytes of one received datagram + * @param onPacket Called as onPacket(src, pgn, data) for each packet; data excludes the CRC + * @return Where and why parsing stopped + */ + template + ParseResult parse_datagram(std::span datagram, Callback &&onPacket) + { + ParseResult result; + while (result.offset < datagram.size()) + { + const std::size_t remaining = datagram.size() - result.offset; + if (remaining < HEADER_SIZE + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + const std::uint8_t *packet = datagram.data() + result.offset; + if ((START_HIGH != packet[0]) || (START_LOW != packet[1])) + { + result.stop = ParseStop::UnknownStart; + return result; + } + + const std::uint8_t src = packet[2]; + const std::uint8_t pgn = packet[3]; + const std::size_t length = packet[4]; + if (remaining < HEADER_SIZE + length + CRC_SIZE) + { + result.stop = ParseStop::Truncated; + return result; + } + + onPacket(src, pgn, datagram.subspan(result.offset + HEADER_SIZE, length)); + result.offset += HEADER_SIZE + length + CRC_SIZE; + result.packets++; + } + return result; + } +} // namespace aog_packet diff --git a/include/app.hpp b/include/app.hpp index 542a4b7..d4d2ab1 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -22,9 +23,13 @@ #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 "tecu_coexistence.hpp" +#include "tractor_facilities.hpp" #include "udp_connections.hpp" class Application @@ -60,6 +65,12 @@ class Application void send_task_controller_status_message(); + /// @brief Keeps the TC off the bus (no TC Status, no TC server update) for TC_QUIET_PERIOD_MS + /// @param reason Logged, so the log shows why clients are about to reconnect + void start_tc_quiet_period(const std::string &reason); + bool is_tc_quiet(); ///< Also ends an expired quiet period + void recover_stale_clients(); + /// @brief Logs AOG's section demand (PGN 229) when it changed, at most every SECTION_DEMAND_LOG_INTERVAL_MS void log_section_demand(); /// @brief Logs when PGN 229 starts and stops, and warns when an implement in automatic is working @@ -71,6 +82,24 @@ class Application bool setup_control_functions(); void setup_task_controller_server(); void setup_tecu_interfaces(); + + /// Our TECU's place among the Tractor ECUs on the bus (ISO 11783-9 4.4.2.1) + enum class TecuRole + { + None, ///< No TECU (disabled, or it could not claim an address) + Primary, ///< Function instance 0 at address 0xF0: sends everything + Secondary ///< Another Tractor ECU is on the bus: we only add what it lacks + }; + /// @brief Create the TECU control function and start its address claim (does not wait for it) + void start_tecu_claim(TecuRole role, std::uint8_t functionInstance); + /// @brief Finishes a pending claim, and once a second checks for other Tractor ECUs: + /// steps down from primary when one appears, and as a secondary sends only what they lack. + void update_tecu(); + /// @brief Destroy the TECU interfaces and control function, so it can be claimed again with another NAME + void teardown_tecu(); + /// @brief Start or stop our TECU messages to match the plan, and advertise them in PGN 65033 + void apply_tecu_plan(const TecuMessagePlan &plan); + void create_speed_messages_interface(const TecuMessagePlan &plan); void setup_udp_connections(); void setup_vt_client(); @@ -89,6 +118,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); @@ -99,6 +130,7 @@ class Application std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; + std::unique_ptr tractorFacilities; std::unique_ptr tecuFunctionalities; std::unique_ptr tcFunctionalities; std::shared_ptr vtClient; @@ -112,14 +144,69 @@ class Application std::uint8_t nmea2000SequenceIdentifier = 0; std::uint32_t lastJ1939SpeedTransmit = 0; std::uint32_t lastTCStatusTransmit = 0; + + /// Longer than the 6 s a client waits for TC Status before it drops the connection and starts over + static constexpr std::uint32_t TC_QUIET_PERIOD_MS = 6500; + /// Minimum time between two quiet periods started for stale clients, so reconnecting clients get time to finish + static constexpr std::uint32_t STALE_CLIENT_RECOVERY_COOLDOWN_MS = 30000; + /// A client still stale after this many quiet periods does not reconnect this way; stop disturbing the others + static constexpr std::uint8_t MAX_STALE_CLIENT_RECOVERIES = 3; + std::uint32_t tcQuietUntilMs = 0; ///< 0 = not quiet + std::uint32_t lastStaleClientRecoveryMs = 0; + std::map staleClientRecoveries; ///< Quiet periods started per client NAME std::int32_t lastSpeedValue = 0; std::int32_t lastXteValue = 0; std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated std::uint32_t lastGnssQualityMs = 0; ///< Timestamp of last PGN 0xD6 fix-quality update (0 = never received) static constexpr std::uint32_t GNSS_QUALITY_TIMEOUT_MS = 2000; ///< No PGN 0xD6 for this long = fix quality unknown + static constexpr std::uint32_t AOG_CONNECTION_TIMEOUT_MS = 3000; ///< No AOG packet for this long = disconnected + + // Guidance track context — real data from AOG PGN 0xF4. + GuidanceTrackProvider trackProvider; + GuidanceTrackContext currentTrackContext; + bool aogWasConnectedForTrack = false; ///< Edge-detection for AOG connect/disconnect transitions + + // Field identity — from AOG PGN 0xF3. Folded into the upper 16 bits of DDI 508 + // (see the PGN 0xF4 handling in setup_udp_connections()) so a track's guidance reference + // line ID is unique across fields, not just within whichever field AOG currently has open. + FieldRegistry fieldRegistry; + std::string currentFieldName; ///< Empty when no field is open + std::uint16_t currentFieldIndex = 0; + bool hasActiveField = false; + std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; + /// @brief AOG opened a field: switch section control on, as if AOG's SC button was pressed (if enabled in settings) + /// @param how What showed the field opening, for the log + void on_aog_field_opened(const std::string &how); + /// The field name (PGN 0xF3) and PGN 229 resuming both report one field opening; act on it once + static constexpr std::uint32_t FIELD_OPEN_DEDUPLICATE_MS = 5000; + std::uint32_t lastFieldOpenAppliedMs = 0; ///< When on_aog_field_opened() last switched section control on, 0 = never + + bool tractorFacilitiesSentOnPowerUp = false; + + static constexpr std::uint32_t TECU_CLAIM_TIMEOUT_MS = 5000; + static constexpr std::uint32_t TECU_CHECK_INTERVAL_MS = 1000; + /// As a secondary, send nothing until the other TECUs answered PGN 65033, or this long has passed + static constexpr std::uint32_t TECU_PLAN_SETTLE_MS = 2500; + /// ...but always watch the bus at least this long, for messages they send without advertising + static constexpr std::uint32_t TECU_PLAN_MIN_SETTLE_MS = 1000; + static constexpr std::uint8_t SECONDARY_TECU_PREFERRED_ADDRESS = 0x80; ///< Arbitrary range; never 0xF0 + static constexpr std::uint8_t ALL_FUNCTION_INSTANCES = 31; ///< Highest NAME function instance (5 bits) + isobus::NAME tecuNameTemplate{ 0 }; ///< Our NAME fields, set in setup_control_functions() + OtherTecuMonitor otherTecus; + TecuRole tecuRole = TecuRole::None; + std::uint8_t tecuFunctionInstance = 0; + std::uint32_t tecuClaimStartedMs = 0; + std::uint32_t tecuClaimedMs = 0; ///< 0 while the claim is pending + bool tecuInterfacesReady = false; + std::uint32_t tecuInterfacesReadyMs = 0; + std::uint32_t lastTecuCheckMs = 0; + bool tecuPlanSettled = false; + TecuMessagePlan tecuPlan; ///< What our TECU sends now + std::set knownOtherTecuAddresses; ///< To log other TECUs appearing and leaving + static constexpr std::uint32_t SECTION_DEMAND_LOG_INTERVAL_MS = 200; static constexpr std::uint32_t SECTION_DEMAND_TIMEOUT_MS = 2000; ///< AOG sends PGN 229 every frame while a field is open static constexpr std::uint32_t WORKING_WITHOUT_DEMAND_WARNING_MS = 10000; diff --git a/include/field_registry.hpp b/include/field_registry.hpp new file mode 100644 index 0000000..13f3720 --- /dev/null +++ b/include/field_registry.hpp @@ -0,0 +1,50 @@ +/** + * @file field_registry.hpp + * @brief Persistent field-name -> field-index mapping, used to make ISOBUS TRACK's + * DDI 508 (Unique Guidance Reference Line ID) actually unique across fields. + * + * AOG's own PGN 0xF4 guidance reference ID is only a 16-bit value scoped to whatever + * field is currently open in AOG - it is not guaranteed unique across different fields. + * An implement that caches per-track state (e.g. an offset) keyed on DDI 508 alone can + * therefore collide across a field switch. This registry assigns each field name a + * stable index, which the caller folds into the upper 16 bits of the 32-bit DDI 508 + * value (see Application's PGN 0xF3/0xF4 handling), leaving AOG's own 16-bit ID in the + * lower 16 bits untouched. + */ + +#pragma once + +#include +#include +#include + +/// @brief Loads/persists a field-name -> field-index mapping from a plain text file. +/// +/// Deliberately its own file rather than part of settings.json: the mapping only grows +/// over time (one line per field ever opened), so a user may want to wipe it on its own +/// (e.g. to reclaim indices) without touching the rest of their configuration. +class FieldRegistry +{ +public: + /// @brief Loads the registry from disk, if present. A missing or unreadable file + /// just starts empty - fields get freshly (re-)indexed and persisted as they're seen. + FieldRegistry(); + + /// @brief Returns the persistent index for a field name, assigning and persisting a + /// new one the first time this name is seen. + /// @param fieldName UTF-8 field folder name, as received from AOG PGN 0xF3. + /// @returns A stable index. Once the 16-bit space is exhausted (65536 distinct + /// field names - far beyond realistic use), the most recently assigned index is + /// reused and a warning is logged, rather than silently colliding with an existing + /// field. + std::uint16_t get_or_assign_index(const std::string &fieldName); + +private: + void load(); + void append_entry(const std::string &fieldName, std::uint16_t index); + + std::string filePath; + std::unordered_map nameToIndex; + std::uint16_t nextIndex = 0; + bool nextIndexExhausted = false; +}; diff --git a/include/guidance_track_context.hpp b/include/guidance_track_context.hpp new file mode 100644 index 0000000..6240e16 --- /dev/null +++ b/include/guidance_track_context.hpp @@ -0,0 +1,85 @@ +/** + * @file guidance_track_context.hpp + * @brief Abstraction layer between AOG input and ISOBUS TRACK (Generation 1) protocol. + * + * This header defines the GuidanceTrackContext struct and the GuidanceTrackProvider + * that consumes AOG PGN 0xF4 guidance-track data. + * + * The ISOBUS TRACK sender consumes a GuidanceTrackContext without caring how it + * was produced. The GuidanceTrackContext maps directly to ISOBUS DDIs 508–511. + */ + +#pragma once + +#include +#include +#include + +/** + * @brief Immutable snapshot of guidance-track state consumed by the ISOBUS TRACK sender. + * + * Corresponds to ISOBUS DDIs: + * - guidanceReferenceLineId -> DDI 508 (Unique Guidance Reference Line ID) + * - actualTrackNumber -> DDI 509 (Actual Guidance Track Number) + * - trackNumberRight -> DDI 510 (Guidance Track Number to the Right) + * - trackNumberLeft -> DDI 511 (Guidance Track Number to the Left) + * - swathWidthMm -> DDI 512 (Guidance Line Swath Width) + */ +struct GuidanceTrackContext +{ + std::uint32_t guidanceReferenceLineId = 1; ///< DDI 508 + std::int32_t actualTrackNumber = 0; ///< DDI 509 — signed; track 0 is valid + std::int32_t trackNumberRight = -1; ///< DDI 510 + std::int32_t trackNumberLeft = 1; ///< DDI 511 + std::uint32_t swathWidthMm = 0; ///< DDI 512 — distance between adjacent tracks; 0 = not reported by AOG + bool valid = false; ///< Context has been initialized with at least one update +}; + +/** + * @brief Real guidance-track provider that consumes AOG PGN 0xF4 (244) data. + * + * PGN 0xF4 payload layout (10 data bytes, optionally 12): + * Byte 0: Sequence counter (0–255, wrapping) + * Byte 1: Flags (bit 0 = valid, bit 1 = heading same way, bit 2 = curve mode) + * Bytes 2-3: Guidance Reference ID (uint16 LE) + * Bytes 4-5: Current Track Number (int16 LE, signed) + * Bytes 6-7: Track Number Left (int16 LE, signed) + * Bytes 8-9: Track Number Right (int16 LE, signed) + * Bytes 10-11: Swath Width in mm (uint16 LE), optional — only present from newer AOG builds + * + * Track numbers (current/left/right) are each shifted by +1 from the raw wire value + * (see AOG_TRACK_NUMBER_OFFSET in parse()) to match the tram-pattern phase implements + * expect — confirmed by field testing, not part of AOG's own documented wire format. + * + * Sequence tracking: + * - Rejects any packet whose sequence number is not strictly ahead of the last + * accepted one (signed delta over the 0-255 wrap), catching both frozen/duplicate + * data and reordered/stale UDP packets arriving out of order. + * - Call reset() after a disconnect to treat the next packet as a fresh start. + * + * Returns valid=true when data is valid, refId != 0, and sequence is fresh. + */ +class GuidanceTrackProvider +{ +public: + /// Minimum payload size (10 data bytes) + static constexpr std::size_t MIN_PAYLOAD_SIZE = 10; + + /// Payload size that includes the swath width + static constexpr std::size_t PAYLOAD_SIZE_WITH_SWATH_WIDTH = 12; + + /** + * @brief Parse AOG PGN 0xF4 payload and produce a GuidanceTrackContext. + * + * @param data Payload bytes (after UDP header stripping) + * @return GuidanceTrackContext with valid=true if parse succeeded and flags indicate valid data + */ + GuidanceTrackContext parse(std::span data); + + /// @brief Reset sequence tracking (e.g., after AOG disconnect timeout). + /// The next parse() call is treated as a fresh start — no delta comparison. + void reset(); + +private: + std::optional lastSequence_; ///< Unset until the first packet is accepted +}; diff --git a/include/settings.hpp b/include/settings.hpp index 06f0278..135608e 100644 --- a/include/settings.hpp +++ b/include/settings.hpp @@ -98,6 +98,18 @@ class Settings */ bool set_vt_enabled(bool enabled, bool save = true); + /// @brief Check whether section control is switched on automatically when an implement connects. + bool is_section_control_on_connect() const; + + /// @brief Set whether section control is switched on automatically when an implement connects. + bool set_section_control_on_connect(bool enabled, bool save = true); + + /// @brief Check whether section control is switched on when AOG opens a field, as if AOG's SC button was pressed. + bool is_section_control_on_field_open() const; + + /// @brief Set whether section control is switched on when AOG opens a field. + bool set_section_control_on_field_open(bool enabled, bool save = true); + /** * @brief Get the configured TC version * @return The configured version (0-4, default 3) @@ -112,6 +124,23 @@ class Settings */ bool set_tc_version(std::uint8_t version, bool save = true); + /** + * @brief Get the configured TC number (ISO 11783-10: function instance + 1) + * @return The TC number (1-32, default 1) + */ + std::uint8_t get_tc_number() const; + + /// @brief Whether the TC number is picked at startup from the numbers free on the bus (tcNumber 0) + bool is_tc_number_automatic() const; + + /** + * @brief Set the TC number. Takes effect after a restart, since it is part of the TC's NAME. + * @param number The TC number (1-32); out of range sets the default + * @param save Whether or not to save the settings to file + * @return True if the setting was set successfully, false otherwise + */ + bool set_tc_number(std::uint8_t number, bool save = true); + /** * @brief Get the configured language code * @return The language code (default "en") @@ -161,7 +190,12 @@ class Settings constexpr static bool DEFAULT_NMEA_SEND_ENABLED = true; constexpr static bool DEFAULT_AOG_HEARTBEAT_ENABLED = true; constexpr static bool DEFAULT_VT_ENABLED = true; + constexpr static bool DEFAULT_SECTION_CONTROL_ON_CONNECT = true; + constexpr static bool DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN = true; constexpr static std::uint8_t DEFAULT_TC_VERSION = 3; ///< SecondEditionDraft (V3 default for maximum implement compatibility) + constexpr static std::uint8_t DEFAULT_TC_NUMBER = 1; ///< TC #1 = function instance 0 + constexpr static std::uint8_t TC_NUMBER_AUTOMATIC = 0; ///< Use the first number no other TC on the bus has + constexpr static std::uint8_t MAX_TC_NUMBER = 32; ///< The NAME function instance has 5 bits (0-31) static const std::string DEFAULT_LANGUAGE_CODE; static const std::string DEFAULT_COUNTRY_CODE; std::array configuredSubnet = DEFAULT_SUBNET; @@ -169,7 +203,10 @@ class Settings bool nmeaSendEnabled = DEFAULT_NMEA_SEND_ENABLED; bool aogHeartbeatEnabled = DEFAULT_AOG_HEARTBEAT_ENABLED; bool vtEnabled = DEFAULT_VT_ENABLED; + bool sectionControlOnConnect = DEFAULT_SECTION_CONTROL_ON_CONNECT; + bool sectionControlOnFieldOpen = DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN; std::uint8_t tcVersion = DEFAULT_TC_VERSION; + std::uint8_t tcNumber = DEFAULT_TC_NUMBER; std::string languageCode = DEFAULT_LANGUAGE_CODE; std::string countryCode = DEFAULT_COUNTRY_CODE; }; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index d1c9ac9..4d94825 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -10,6 +10,7 @@ #pragma once #include "element_hierarchy.hpp" +#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" @@ -18,7 +19,9 @@ #include #include #include +#include #include +#include constexpr std::uint8_t NUMBER_SECTIONS_PER_CONDENSED_MESSAGE = 16; @@ -30,6 +33,16 @@ enum SectionState : std::uint8_t NOT_INSTALLED = 3 ///< Section is not installed }; +/// @brief Track control levels, as the bits an implement sets in DDI 505 for the levels it supports. +/// Only Level 1 is implemented by the TC. +enum class TrackControlLevel : std::uint8_t +{ + None = 0, + Level1 = 1, + Level2 = 2, + Level3 = 4 +}; + class ClientState { public: @@ -56,6 +69,10 @@ class ClientState isobus::DeviceDescriptorObjectPool &get_pool(); bool are_measurement_commands_sent() const; void mark_measurement_commands_sent(); + /// @brief Schedules the TC's section control state to be applied to this client at the given time. + void schedule_initial_section_control(std::uint32_t dueMs); + /// @brief Returns true (once) when the scheduled initial section control state is due. + bool take_due_initial_section_control(std::uint32_t nowMs); std::uint16_t get_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const; void set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber); bool has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const; @@ -66,9 +83,25 @@ class ClientState void set_element_work_state(std::uint16_t elementNumber, bool isWorking); bool try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const; + /// @brief Advances the DDI 507 sequence number if the track or reference line differs from the + /// last one announced to this client, and remembers the new values. + /// @returns The sequence number to announce. + std::uint32_t update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId); + + // Track control level negotiation. The implement reports the levels it supports in DDI 505, + // the TC answers by writing the level it wants to use to DDI 506, and the implement's echo of + // that completes the negotiation. + int get_supported_track_control_levels() const; + void set_supported_track_control_levels(int levels); + bool is_track_control_level_sent() const; + void set_track_control_level_sent(bool sent); + bool is_track_negotiation_complete() const; + void set_track_negotiation_complete(bool complete); + private: isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC bool areMeasurementCommandsSent = false; ///< Whether or not the measurement commands have been sent + std::optional initialSectionControlDueMs; ///< When to apply the TC's section control state after connect std::map ddiToElementNumber; ///< Mapping of DDI to element number // TODO: better way to do this? std::uint8_t numberOfSections; @@ -83,6 +116,12 @@ class ClientState bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting bool usesPerElementControl = false; ///< Legacy mode: use per-element setpoint instead of condensed std::uint16_t perElementSetpointDDI = 0; ///< The DDI to use for per-element setpoints (289 or 141), 0 if not applicable + std::int32_t lastSentTrackNumber = 0; ///< Last track number announced, for DDI 507 change detection + std::uint32_t lastSentReferenceLineId = 0; ///< Last reference line ID announced, for DDI 507 change detection + std::uint32_t guidanceTrackSequenceNumber = 0; ///< Per-client DDI 507 sequence number + int supportedTrackControlLevels = 0; ///< Raw DDI 505 bitmask from the implement (bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3) + bool trackControlLevelSent = false; ///< Whether DDI 506 has been written + bool trackNegotiationComplete = false; ///< Whether the implement confirmed the level written to DDI 506 }; // Create the task controller server object, this will handle all the ISOBUS communication for us @@ -120,17 +159,52 @@ class MyTCServer : public isobus::TaskControllerServer void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + /// @brief Sets the section control state applied to implements as they connect, before AOG asks for one. + void set_initial_section_control_enabled(bool enabled); + /// @brief Sends GNSS quality (DDI 514) to every client that declares that DDI. /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated void send_gnss_quality(std::uint8_t quality); + /// @brief Starts watching for clients that talk to us as if connected although we do not know them. + /// See take_stale_clients(). Call once, after initialize(). + void start_stale_client_detection(); + + /// @brief Returns the clients that sent us a Client Task or Value message since the last call without + /// being one of our active clients. Such a client still believes it is connected (typically because the + /// TC restarted faster than the client's 6 s TC Status timeout) and the stack ignores or NACKs it until + /// it reconnects, which it will not do while it keeps seeing our TC Status. + /// Call from the thread that calls update(). + std::vector> take_stale_clients(); + + /// @brief Drops the stale client candidates collected so far, e.g. ones that arrived while the TC was quiet + void clear_stale_clients(); + + ~MyTCServer() override; + + /// @brief Announces the current guidance track (DDI 507-511) and line deviation (DDI 513) to every + /// client that declares those DDIs. Does nothing while the context is not valid. + /// @param ctx Current guidance track state from AOG + /// @param lineDeviationMm Deviation from the guidance line in mm + void send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm); + + /// @brief Writes the track control state (DDI 515) to every client that has finished negotiating + /// a track control level and declares that DDI. + /// @param enabled true = automatic, false = manual/off + void update_track_control_enabled(bool enabled); + private: + static void on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer); + void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); + /// @brief Sets the local auto/manual flag and commands DDI 160 if the client supports it. Caller must hold clientsMutex. + void apply_section_control_state(std::shared_ptr client, ClientState &state, bool enabled); bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); std::map, ClientState> clients; std::map, std::queue>> uploadedPools; + bool desiredSectionControlEnabled = false; ///< Applied to implements when they connect; follows AOG's ISOBUS SC button. Guarded by clientsMutex /// @brief Guards clients and uploadedPools. /// @@ -150,4 +224,10 @@ class MyTCServer : public isobus::TaskControllerServer /// update_section_states -> send_section_setpoint_states), and get_clients() /// must keep returning a copy, not a reference (see its declaration above). mutable std::recursive_mutex clientsMutex; + + /// @brief Sources of Client Task / Value messages to us, collected on the CAN thread and checked + /// against the active clients in take_stale_clients(). Guarded by staleCandidatesMutex. + std::vector> staleCandidates; + std::mutex staleCandidatesMutex; + bool staleClientDetectionStarted = false; }; diff --git a/include/tecu_coexistence.hpp b/include/tecu_coexistence.hpp new file mode 100644 index 0000000..ee0226c --- /dev/null +++ b/include/tecu_coexistence.hpp @@ -0,0 +1,153 @@ +/** + * @brief Sharing the implement bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1). + * + * When several Tractor ECUs are on the implement bus, the one with function + * instance 0 is the primary. A Tractor ECU with a higher function instance + * "shall not provide any messages which are provided by a Tractor ECU with a + * lower function instance", requests the tractor facility response (PGN 65033) + * from those, and "shall only set those facilities bit(s) that are not available + * in the other Tractor ECUs". + * + * Our TECU runs as the primary when it is alone, and as a secondary otherwise. + * This file holds the decision of what a secondary sends (pure functions, unit + * tested) and OtherTecuMonitor, which collects what the other TECUs advertise + * and actually send. + */ + +#pragma once + +#include "tractor_facilities.hpp" + +#include "isobus/isobus/can_control_function.hpp" +#include "isobus/isobus/can_message.hpp" +#include "isobus/isobus/can_message_frame.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// Messages our TECU can put on the bus +enum class TecuMessage : std::uint8_t +{ + GroundBasedSpeed, ///< PGN 65097, ground-based speed, distance and direction + WheelBasedSpeed, ///< PGN 65096, wheel-based speed, distance and direction + MachineSelectedSpeed, ///< PGN 61474 + VehicleDirectionSpeed, ///< PGN 65256, J1939 vehicle direction/speed (no facility bit) + CogSogRapidUpdate, ///< PGN 129026, NMEA 2000 COG & SOG rapid update (no facility bit) + TimeDate, ///< PGN 65254 + Count +}; + +constexpr std::size_t TECU_MESSAGE_COUNT = static_cast(TecuMessage::Count); + +/// @brief The PGN a TecuMessage is sent with +std::uint32_t tecu_message_pgn(TecuMessage message); + +/// @brief Short name for logs, e.g. "ground-based speed (PGN 65097)" +std::string tecu_message_name(TecuMessage message); + +/// Which of our messages to send +struct TecuMessagePlan +{ + std::array send{}; + + bool sends(TecuMessage message) const + { + return send[static_cast(message)]; + } + bool operator==(const TecuMessagePlan &other) const + { + return send == other.send; + } + bool operator!=(const TecuMessagePlan &other) const + { + return send != other.send; + } +}; + +/// @brief Decide which messages our TECU sends. +/// @param isPrimary True when our TECU is the primary (function instance 0): it sends everything. +/// @param advertisedByOthers Facilities the other (lower instance) TECUs report in PGN 65033, combined. +/// @param sentByOthers Messages seen recently from another TECU, whether advertised or not. +/// @returns As a secondary, only the messages no other TECU provides (ISO 11783-9 4.4.2.1 c). +TecuMessagePlan plan_tecu_messages(bool isPrimary, + const Facilities &advertisedByOthers, + const std::array &sentByOthers); + +/// @brief The facilities to advertise for a plan (ISO 11783-9 4.4.2.1 d): exactly the messages we send. +/// The time/date bit is left clear; it is set separately while time/date is actually provided. +Facilities facilities_for_plan(const TecuMessagePlan &plan); + +/// @brief Bitwise OR of two facility sets (what any of several TECUs provides) +Facilities merge_facilities(const Facilities &a, const Facilities &b); + +/// @brief The J1939 PGN of a 29-bit CAN identifier (PDU1 destination address removed) +std::uint32_t pgn_from_can_identifier(std::uint32_t identifier); + +/// Another Tractor ECU on the bus +struct OtherTecu +{ + std::shared_ptr controlFunction; + std::uint8_t address = 0xFE; + std::uint8_t functionInstance = 0; + std::uint16_t manufacturerCode = 0; +}; + +/// @brief Tracks the other Tractor ECUs: who they are, the facilities they advertise and what they send. +/// +/// refresh() and the queries run on the main thread. on_frame() and the PGN 65033 callback run on the +/// CAN stack's background thread (see docs/CONCURRENCY.md), so the shared state is atomic or under a mutex. +/// The object lives as long as the Application: the stack may still call a global PGN callback once +/// after it was removed, so this must never be destroyed while the stack runs. +class OtherTecuMonitor +{ +public: + /// How long a message seen from another TECU counts as "sent by it" + static constexpr std::uint32_t SENT_BY_OTHERS_WINDOW_MS = 3000; + /// The same for time/date, which may only appear when something requests it + static constexpr std::uint32_t TIME_DATE_SENT_BY_OTHERS_WINDOW_MS = 30000; + + /// @brief Register the PGN 65033 listener. Call once. + void initialize(); + + /// @brief Re-read the other TECUs from the network manager, excluding our own control functions. + /// Only TECUs with a function instance up to maxFunctionInstance count: a secondary defers to lower + /// instances only (ISO 11783-9 4.4.2.1 c), so two secondaries never wait on each other. Forgets + /// facilities of TECUs that have gone. + /// @returns Those TECUs currently on the bus, lowest function instance first. + std::vector refresh(std::uint8_t maxFunctionInstance); + + /// @brief Called for every received CAN frame (background thread) + void on_frame(const isobus::CANMessageFrame &frame); + + /// @brief The facilities advertised by the other TECUs, combined + Facilities advertised_facilities() const; + + /// @brief Whether every other TECU has answered with PGN 65033 + bool all_facilities_received() const; + + /// @brief Messages seen from another TECU within SENT_BY_OTHERS_WINDOW_MS (time/date: its own window) + std::array sent_by_others() const; + + /// @brief Whether we already asked this address for PGN 65033 since it appeared + bool facilities_requested(std::uint8_t address) const; + void mark_facilities_requested(std::uint8_t address); + +private: + static void on_facilities_response(const isobus::CANMessage &message, void *parent); + + std::array, 256> isOtherTecuAddress{}; + std::array, TECU_MESSAGE_COUNT> lastSentByOthersMs{}; ///< 0 = never + + mutable std::mutex mutex; ///< Guards the members below + std::map facilitiesByAddress; + std::set otherAddresses; + std::set requestedAddresses; + std::map nameByAddress; ///< To notice a different TECU taking over an address +}; diff --git a/include/tractor_facilities.hpp b/include/tractor_facilities.hpp new file mode 100644 index 0000000..34b14c7 --- /dev/null +++ b/include/tractor_facilities.hpp @@ -0,0 +1,194 @@ +/** + * @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 +#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 The minutes to add to UTC to get local time, positive east of Greenwich. +/// Compares the two broken-down clocks, so it includes daylight saving time and needs no time zone lookup. +int utc_offset_minutes(const std::tm &utc, const std::tm &local); + +/// @brief Encode the Time/Date payload (PGN 65254, ISO 11783-7 B.1) from the same instant as UTC and local time. +/// @param milliseconds Milliseconds into the current second; the message has a resolution of 250 ms. +std::array encode_time_date(const std::tm &utc, const std::tm &local, int milliseconds); + +/// @brief Implements ISO 11783-7 Tractor Facilities response (PGN 65033) +/// and Required Tractor Facilities diagnostic logging (PGN 65032). +class TractorFacilities +{ +public: + static constexpr std::uint32_t PGN_TRACTOR_FACILITIES = 65033; ///< 0xFE09 + static constexpr std::uint32_t PGN_REQUIRED_TRACTOR_FACILITIES = 65032; ///< 0xFE08 + static constexpr std::uint32_t PGN_TIME_DATE = 65254; ///< 0xFEE6 + + TractorFacilities(std::shared_ptr tecuCF, + std::shared_ptr settings); + + /// @brief Removes the callbacks registered by initialize(), so the TECU can be rebuilt at runtime + ~TractorFacilities(); + + TractorFacilities(const TractorFacilities &) = delete; + TractorFacilities &operator=(const TractorFacilities &) = delete; + + /// @brief Register the PGN 65033 request callback and the PGN 65032 + /// global receive listener. Must be called after the TECU + /// address claim has completed. + 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 our TECU provides time/date (PGN 65254 / FEE6). + /// When true, it answers requests for that PGN and advertises the time/date facility bit in PGN 65033. + /// When false, it leaves requests to the other Tractor ECU. It is never sent unrequested (ISO 11783-7 B.1). + void set_time_date_active(bool active); + + /// @brief Advertise exactly these facilities (the time/date bit still follows set_time_date_active()). + /// Used when our TECU shares the bus with other Tractor ECUs and only provides what they lack + /// (ISO 11783-9 4.4.2.1). Until this is called, the facilities follow the speed interface. + void set_provided_facilities(const Facilities &facilities); + + /// @brief Build the 8-byte payload from the current runtime state. + std::array build_payload() const; + +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 PGN-request callback registered for PGN 65254 (Time/Date). + static bool on_time_date_request(std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + isobus::AcknowledgementType &acknowledgeType, + void *parentPointer); + + /// @brief Static global-PGN callback for PGN 65032 (Required Tractor + /// Facilities). Diagnostic only — logs the request unconditionally + /// (not gated by log level) and never changes our response. + 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 provide PGN 65254 (FEE6). Atomic because set_time_date_active() + /// runs on the main thread, while the PGN request callbacks that read it run + /// on the CAN stack's thread (see docs/CONCURRENCY.md). + std::atomic timeDateActive{ false }; + std::atomic timeDateRequestLogged{ false }; ///< To log only the first request answered + + /// Set by set_provided_facilities() on the main thread, read by build_payload() from PGN request callbacks + mutable std::mutex providedFacilitiesMutex; + bool hasProvidedFacilities = false; + Facilities providedFacilities; + bool initialized = false; + + /// Source addresses for which we have already logged an info-level + /// "request received" line, to avoid per-message spam. + std::set loggedRequesters; +}; diff --git a/include/udp_connections.hpp b/include/udp_connections.hpp index 2c2200a..36d2032 100644 --- a/include/udp_connections.hpp +++ b/include/udp_connections.hpp @@ -10,7 +10,9 @@ #pragma once #include +#include #include +#include #include "settings.hpp" using boost::asio::ip::udp; @@ -92,7 +94,16 @@ class UdpConnections */ std::uint8_t calculate_crc(std::span data); + /** + * @brief Log a packet that does not start with PACKET_START, once even if both sockets receive it + * @param sender The endpoint the packet came from + * @param data The bytes from the unexpected start onwards + */ + void log_unknown_start(const udp::endpoint &sender, std::span data); + PacketCallback packetCallback = nullptr; + std::string lastUnknownStartSignature; ///< Sender and leading bytes of the last logged unknown start + std::chrono::steady_clock::time_point lastUnknownStartTime; std::shared_ptr settings; udp::socket udpConnection; udp::socket udpConnectionAddressDetection; diff --git a/readme.md b/readme.md index 8c67159..91dc4ed 100644 --- a/readme.md +++ b/readme.md @@ -60,11 +60,14 @@ AOG-TaskController reads its configuration from a `settings.json` file located i | Key | Type | Default | Description | |---|---|---|---| | `subnet` | `int[3]` | `[192, 168, 5]` | LAN prefix used to select the AOG-facing NIC and broadcast address. | -| `tecuEnabled` | `bool` | `true` | Enables the internal Tractor ECU. Disable it when the tractor already has a TECU. A change made from the VT takes effect after restart. | -| `nmeaSendEnabled` | `bool` | `true` | Enables cyclic NMEA2000 COG/SOG transmission. Requires `tecuEnabled`. | +| `tecuEnabled` | `bool` | `true` | `true`: AOG is the tractor's Tractor ECU (address 240), and steps back if the tractor turns out to have one. `false`: use when the tractor already has a TECU; AOG then only adds what it lacks, such as ground-based speed from GNSS (ISO 11783-9 §4.4.2.1). A change made from the VT takes effect after restart. | +| `nmeaSendEnabled` | `bool` | `true` | Enables cyclic NMEA2000 COG/SOG transmission (not while the tractor's TECU sends it). | | `aogHeartbeatEnabled` | `bool` | `true` | Sends the 100 ms zero-section heartbeat when no implement is connected. Disable for AgOpenGPS versions before v6.8.2 beta 5. | | `vtEnabled` | `bool` | `true` | Enables the ISOBUS Virtual Terminal UI. | +| `sectionControlOnConnect` | `bool` | `true` | Switches section control on 2 s after an implement connects, as if AOG's SC button was pressed. | +| `sectionControlOnFieldOpen` | `bool` | `true` | Switches section control on when AOG opens a field (field name, PGN 0xF3, or section states, PGN 229, starting after a gap), as if AOG's SC button was pressed. | | `tcVersion` | `integer` | `3` | Task Controller version code, from `0` through `4`; `3` is Second Edition Draft. | +| `tcNumber` | `integer` | `1` | Task Controller number, `1`–`32` (ISO 11783-10: the TC NAME's function instance + 1). Set `2` or higher when the tractor already has a TC, so implements can tell the two apart. `0` picks the first number no other TC on the bus has, which adds about 1.4 s to startup. Takes effect after restart. | | `languageCode` | `string` | `"en"` | Two-character language code advertised on ISOBUS. | | `countryCode` | `string` | `"US"` | Two-character country code advertised on ISOBUS. | @@ -78,6 +81,7 @@ AOG-TaskController reads its configuration from a `settings.json` file located i "aogHeartbeatEnabled": true, "vtEnabled": true, "tcVersion": 3, + "tcNumber": 1, "languageCode": "en", "countryCode": "US" } @@ -95,6 +99,14 @@ The application logs the detected VT version, screen size, softkey dimensions, a - **Maximum Booms:** 1 - **Maximum Sections:** 64 (supports both individual sections and zone-based control) - **Section Control:** Generation 1 (TC-SC) with support for DDI 160/161/290 +- **Track Control:** Level 1 — negotiates the level with the implement (DDI 505/506) and switches its track control on and off (DDI 515) together with AgOpenGPS's Auto command + +### Guidance data sent to implements + +Sent to any implement whose device description declares the DDI as settable. + +- **GNSS quality (DDI 514):** AgOpenGPS's GPS fix quality, every 250 ms. +- **Guidance track (DDI 507-513):** the current track number, the tracks to its left and right, the track spacing, a reference line ID that is unique across fields, and the deviation from the guidance line, every 250 ms while AgOpenGPS has an active track. ### Guidance data sent to implements diff --git a/src/app.cpp b/src/app.cpp index a697e8b..8ff0b93 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,14 +22,18 @@ #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 #include #include #include @@ -44,6 +49,95 @@ static std::string format_hex_address(std::uint8_t address) return value.str(); } +// Requests to Send with a global destination are invalid and ignored by the stack. Its per-message warning +// has no source address, so count them per source here (CAN thread) and report once per minute from update(). +static std::array, 256> globalRtsCountBySource{}; + +static void count_global_rts(const isobus::CANMessageFrame &frame) +{ + constexpr std::uint8_t TP_CONNECTION_MANAGEMENT_PF = 0xEC; + constexpr std::uint8_t TP_REQUEST_TO_SEND = 16; + if (frame.isExtendedFrame && (frame.dataLength > 0) && + (((frame.identifier >> 16) & 0xFF) == TP_CONNECTION_MANAGEMENT_PF) && + (((frame.identifier >> 8) & 0xFF) == 0xFF) && + (frame.data[0] == TP_REQUEST_TO_SEND)) + { + globalRtsCountBySource[frame.identifier & 0xFF]++; + } +} + +static void report_global_rts_counts() +{ + std::ostringstream sources; + std::uint32_t total = 0; + for (std::size_t source = 0; source < globalRtsCountBySource.size(); source++) + { + const std::uint32_t count = globalRtsCountBySource[source].exchange(0); + if (count > 0) + { + sources << (total > 0 ? ", " : "") << format_hex_address(static_cast(source)) << " (" << count << ")"; + total += count; + } + } + if (total > 0) + { + log("TP") << "Ignored " << total << " Request to Send message(s) with a global destination in the last minute, from source address(es) " << sources.str() << std::endl; + } +} + +// 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) { @@ -152,6 +246,50 @@ static bool check_tc_address_conflict(const std::shared_ptr tcInstancesSeen{}; + +// Reads the claims straight from the frames. The stack's table isn't enough: its roll-call after our +// request can mark a CF offline that has just answered, depending on thread timing. +static void record_tc_claim(const isobus::CANMessageFrame &frame) +{ + if ((8 != frame.dataLength) || (static_cast(isobus::CANLibParameterGroupNumber::AddressClaim) != isobus::CANIdentifier(frame.identifier).get_parameter_group_number())) + { + return; + } + + std::uint64_t rawName = 0; + for (std::size_t i = 0; i < 8; i++) + { + rawName |= static_cast(frame.data[i]) << (8 * i); + } + + const isobus::NAME name(rawName); + if ((static_cast(isobus::NAME::Function::TaskController) == name.get_function_code()) && (name.get_function_instance() < tcInstancesSeen.size())) + { + tcInstancesSeen[name.get_function_instance()] = true; + } +} + +// Lowest function instance not claimed by a TC during discovery, if there is one. +static std::optional find_free_tc_function_instance() +{ + std::optional freeInstance; + for (std::uint8_t instance = 0; instance < tcInstancesSeen.size(); instance++) + { + if (tcInstancesSeen[instance]) + { + log("TC Discovery") << "Existing Task Controller with function instance " << static_cast(instance) << std::endl; + } + else if (!freeInstance) + { + freeInstance = instance; + } + } + return freeInstance; +} + Application::Application(std::shared_ptr canDriver) : canDriver(canDriver) { @@ -194,6 +332,7 @@ bool Application::setup_can_hardware() } isobus::CANHardwareInterface::set_number_of_can_channels(1); isobus::CANHardwareInterface::assign_can_channel_frame_handler(0, canDriver); + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener(count_global_rts); if ((!isobus::CANHardwareInterface::start()) || (!canDriver->get_is_valid())) { @@ -204,14 +343,32 @@ bool Application::setup_can_hardware() isobus::CANNetworkManager::CANNetwork.get_configuration().set_number_of_packets_per_cts_message(255); + // A TC that claimed its address before we started says nothing about its NAME unless asked + const bool discoverTcNumbers = settings->is_tc_number_automatic(); + isobus::EventCallbackHandle tcClaimListener = 0; + if (discoverTcNumbers) + { + tcClaimListener = isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener(record_tc_claim); + } + if (discoverTcNumbers && !isobus::CANNetworkManager::CANNetwork.send_request_for_address_claim(0)) + { + log("TC Discovery") << "Could not request address claims, TCs already on the bus may be missed" << std::endl; + } + // Start CAN network and allow a brief update window to observe bus CFs + const std::uint32_t busDiscoveryWindowMs = discoverTcNumbers ? 1500 : 100; auto busDiscoveryWindowStart = isobus::SystemTiming::get_timestamp_ms(); - while (isobus::SystemTiming::get_time_elapsed_ms(busDiscoveryWindowStart) < 100) + while (isobus::SystemTiming::get_time_elapsed_ms(busDiscoveryWindowStart) < busDiscoveryWindowMs) { isobus::CANNetworkManager::CANNetwork.update(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + if (discoverTcNumbers) + { + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().remove_listener(tcClaimListener); + } + return true; } @@ -228,19 +385,32 @@ bool Application::setup_control_functions() ourNAME.set_device_class(0); ourNAME.set_identity_number(20); ourNAME.set_ecu_instance(0); - ourNAME.set_function_instance(0); // TC #1. If you want to change the TC number, change this. + ourNAME.set_function_instance(0); ourNAME.set_device_class_instance(0); ourNAME.set_manufacturer_code(1407); isobus::NAME tcNAME = ourNAME; tcNAME.set_function_code(static_cast(isobus::NAME::Function::TaskController)); + // ISO 11783-10: the TC number is the TC's NAME function instance + 1 (TC #1 = instance 0) + std::uint8_t tcNumber = settings->get_tc_number(); + if (settings->is_tc_number_automatic()) + { + const auto freeInstance = find_free_tc_function_instance(); + if (!freeInstance) + { + log("TC Config") << "No free TC number available" << std::endl; + return false; + } + tcNumber = static_cast(*freeInstance + 1); + log("TC Config") << "Automatically selected TC number " << static_cast(tcNumber) << std::endl; + } + tcNAME.set_function_instance(static_cast(tcNumber - 1)); - isobus::NAME tecuNAME = ourNAME; - tecuNAME.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); - tecuNAME.set_arbitrary_address_capable(false); // TECU address is fixed - tecuNAME.set_ecu_instance(0); + tecuNameTemplate = ourNAME; + tecuNameTemplate.set_function_code(static_cast(isobus::NAME::Function::TractorECU)); + tecuNameTemplate.set_ecu_instance(0); - log("Init") << "Creating Task Controller control function..." << std::endl; + log("Init") << "Creating Task Controller control function (TC number " << static_cast(tcNumber) << ")..." << std::endl; tcCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tcNAME, 0, isobus::preferred_addresses::IndustryGroup2::TaskController_MappingComputer); // The preferred address for a TC is defined in ISO 11783 // Wait for TC address claim with bounded wait loop (no async to avoid blocking on destruction) @@ -287,62 +457,70 @@ bool Application::setup_control_functions() } } - // Create TECU control function - // TODO: Should we wait between this and TC? - // TODO: If there's already a TECU on the bus we should not create ours - if (tcCF && settings->is_tecu_enabled()) - { // Only create TECU if TC was created and ECU is enabled - log("Init") << "Creating Tractor ECU control function..." << std::endl; - tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(tecuNAME, 0, isobus::preferred_addresses::IndustryGroup2::TractorECU); - - // Wait for TECU address claim with minimum 250ms delay per J1939-81 section 4.4.4.1 - log("Init") << "Tractor ECU control function created, waiting for address claim..." << std::endl; + // Create TECU control function. By now the TC's own address claim made every CF on the bus + // announce itself, so a tractor's Tractor ECU is already known (ISO 11783-9 4.4.2.1). + // tecuEnabled on: AOG is the tractor's TECU (primary at 0xF0), stepping down if one turns up anyway. + // tecuEnabled off ("the tractor has its own TECU"): a secondary that fills the gaps, never at 0xF0; + // with no other Tractor ECU on the bus, every message is a gap. + if (tcCF) + { // Only create TECU if TC was created + otherTecus.initialize(); + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher().add_listener([this](const isobus::CANMessageFrame &frame) { + otherTecus.on_frame(frame); // CAN thread: notes what the other TECUs send + }); + + const auto others = otherTecus.refresh(ALL_FUNCTION_INSTANCES); + for (const auto &other : others) + { + knownOtherTecuAddresses.insert(other.address); + log("Init") << "Tractor ECU already on the bus at " << format_hex_address(other.address) << " (function instance " + << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + } + if (settings->is_tecu_enabled() && others.empty()) + { + start_tecu_claim(TecuRole::Primary, 0); + } + else + { + if (!settings->is_tecu_enabled()) + { + log("Init") << "tecuEnabled is off: the TECU only fills in what the tractor's Tractor ECU does not provide" << std::endl; + } + const int instance = others.empty() ? 1 : std::min(others.back().functionInstance + 1, ALL_FUNCTION_INSTANCES); + start_tecu_claim(TecuRole::Secondary, static_cast(instance)); + } - // Update the network manager to process TECU CF claiming - int tecuClaimAttempts = 0; - const int MAX_TECU_CLAIM_ATTEMPTS = 50; // 5 seconds max - while (!tecuCF->get_address_valid() && tecuClaimAttempts < MAX_TECU_CLAIM_ATTEMPTS) + // Wait for the claim here at startup, as before, so the TECU interfaces exist once setup is done. + // At runtime update_tecu() finishes claims without blocking. + while (!tecuCF->get_address_valid() && !isobus::SystemTiming::time_expired_ms(tecuClaimStartedMs, TECU_CLAIM_TIMEOUT_MS)) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); isobus::CANNetworkManager::CANNetwork.update(); - tecuClaimAttempts++; } - // TECU is non-arbitrary-address-capable: it must land on its preferred address or not at all - if (tecuCF->get_address_valid() && tecuCF->get_address() == isobus::preferred_addresses::IndustryGroup2::TractorECU) + if (tecuCF->get_address_valid()) { // Record when the address was actually claimed for the 250ms delay calculation - auto tecuAddressClaimedTime = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = (0 == tecuClaimedMs) ? 1 : tecuClaimedMs; log("Init") << "TECU claimed address " << static_cast(tecuCF->get_address()) << std::endl; // Ensure minimum 250ms delay after address claim per J1939-81 - auto tecuClaimElapsedMs = isobus::SystemTiming::get_time_elapsed_ms(tecuAddressClaimedTime); - if (tecuClaimElapsedMs < MINIMUM_ADDRESS_CLAIM_DELAY_MS) + log("Init") << "Waiting " << MINIMUM_ADDRESS_CLAIM_DELAY_MS << "ms after TECU address claim (J1939-81 250ms rule)..." << std::endl; + auto delayStart = isobus::SystemTiming::get_timestamp_ms(); + while (isobus::SystemTiming::get_time_elapsed_ms(delayStart) < MINIMUM_ADDRESS_CLAIM_DELAY_MS) { - auto remainingDelay = MINIMUM_ADDRESS_CLAIM_DELAY_MS - tecuClaimElapsedMs; - log("Init") << "Waiting " << remainingDelay << "ms after TECU address claim (J1939-81 250ms rule)..." << std::endl; - // Process CAN messages during the delay to prevent timeouts - auto delayStart = isobus::SystemTiming::get_timestamp_ms(); - while (isobus::SystemTiming::get_time_elapsed_ms(delayStart) < remainingDelay) - { - isobus::CANNetworkManager::CANNetwork.update(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } + isobus::CANNetworkManager::CANNetwork.update(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + // A primary that did not get 0xF0 lost it to another device: update_tecu() steps it down } else { tecuAddressClaimFailed = true; - if (tecuCF->get_address_valid()) - { - log("Warning") << "TECU claimed unexpected address " << static_cast(tecuCF->get_address()) << " instead of " << static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU) << "!" << std::endl; - } - else - { - log("Warning") << "TECU failed to claim address " << static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU) << "! Another TECU may be on the bus." << std::endl; - } - log("Warning") << "TECU functionality will be disabled." << std::endl; - tecuCF.reset(); // Release the failed control function + log("Warning") << "TECU failed to claim an address. TECU functionality will be disabled." << std::endl; + teardown_tecu(); + tecuRole = TecuRole::None; } } @@ -381,6 +559,11 @@ void Application::setup_task_controller_server() languageInterface.set_language_code(settings->get_language_code()); languageInterface.set_country_code(settings->get_country_code()); tcServer->initialize(); + tcServer->set_initial_section_control_enabled(settings->is_section_control_on_connect()); + tcServer->start_stale_client_detection(); + // An implement still connected to a previous run of the TC (a restart takes only a few seconds) would keep + // talking to us without reconnecting, and get NACKed, unless it sees our TC Status stop for longer than 6 s. + start_tc_quiet_period("startup"); tcServer->set_task_totals_active(true); // TODO: make this dynamic based on status in AOG tcFunctionalities = std::make_unique(tcCF); tcFunctionalities->set_functionality_is_supported( @@ -399,13 +582,21 @@ void Application::setup_task_controller_server() 1, true); tcFunctionalities->set_task_controller_section_control_server_option_state(1, 64); - log("Init") << "TC announced TC-BAS and TC-SC (1 boom / 64 sections) via PGN 64654" << std::endl; + + // Announce the Task Controller TRACK Server (functionality 27), telling implements this TC + // negotiates a track control level. AgIsoStack has no enumerator for it yet, so cast the value. + constexpr auto TASK_CONTROLLER_TRACK_SERVER = static_cast(27); + tcFunctionalities->set_functionality_is_supported(TASK_CONTROLLER_TRACK_SERVER, 1, true); + + log("Init") << "TC announced TC-BAS, TC-SC (1 boom / 64 sections) and TC-TRACK via PGN 64654" << std::endl; } void Application::setup_tecu_interfaces() { - // Initialize speed and distance messages - if (tecuCF && tecuCF->get_address_valid()) + // Initialize speed and distance messages. A primary that did not get 0xF0 waits for update_tecu() + // to step it down instead of sending from another address. + if (tecuCF && tecuCF->get_address_valid() && + ((TecuRole::Primary != tecuRole) || (isobus::preferred_addresses::IndustryGroup2::TractorECU == tecuCF->get_address()))) { log("Init") << "Creating TECU Control Function Functionalities..." << std::endl; tecuFunctionalities = std::make_unique(tecuCF); @@ -419,30 +610,312 @@ void Application::setup_tecu_interfaces() true); log("Init") << "TECU announced as Class 1 Tractor ECU (PGN 64654)" << std::endl; + // The primary sends everything. A secondary sends nothing until it knows what the other + // Tractor ECUs provide (update_tecu()), so it never doubles one of their messages. + const TecuMessagePlan plan = (TecuRole::Primary == tecuRole) ? plan_tecu_messages(true, Facilities{}, {}) : TecuMessagePlan{}; + log("Init") << "Creating Speed Messages Interface on TECU..." << std::endl; - speedMessagesInterface = std::make_unique(tecuCF, true, true, true, false); //TODO: make configurable whether to send these messages - speedMessagesInterface->initialize(); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); - speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); - speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + create_speed_messages_interface(plan); log("Init") << "Speed Messages Interface created and initialized." << std::endl; log("Init") << "Creating NMEA2000 Message Interface on TECU..." << std::endl; - nmea2000MessageInterface = std::make_unique(tecuCF, settings->is_nmea_send_enabled(), false, false, false, false, false, false); + nmea2000MessageInterface = std::make_unique( + tecuCF, settings->is_nmea_send_enabled() && plan.sends(TecuMessage::CogSogRapidUpdate), false, false, false, false, false, false); nmea2000MessageInterface->initialize(); log("Init") << "NMEA2000 Message Interface created and initialized." << std::endl; + + // 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->set_provided_facilities(facilities_for_plan(plan)); + tractorFacilities->set_time_date_active(plan.sends(TecuMessage::TimeDate)); + tractorFacilities->initialize(); + tecuPlan = plan; + tecuInterfacesReady = true; + tecuInterfacesReadyMs = isobus::SystemTiming::get_timestamp_ms(); + + // Register repetition-rate diagnostic on the TECU's PGN request protocol + auto tecuPgnReq = tecuCF->get_pgn_request_protocol().lock(); + if (tecuPgnReq) + { + tecuPgnReq->register_request_for_repetition_rate_callback( + static_cast(isobus::CANLibParameterGroupNumber::Any), &log_repetition_rate_request, nullptr); + } + + // Also catch broadcast PGN 0xCC00 messages that AgIsoStack warns about. + // Once only: this runs again whenever the TECU is rebuilt (update_tecu()). + static bool broadcastRepetitionRateListenerAdded = false; + if (!broadcastRepetitionRateListenerAdded) + { + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + 0xCC00 /* RequestForRepetitionRate */, &log_broadcast_repetition_rate, nullptr); + broadcastRepetitionRateListenerAdded = true; + } } else { - if (!settings->is_tecu_enabled()) + log("Warning") << "TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; + } +} + +void Application::start_tecu_claim(TecuRole role, std::uint8_t functionInstance) +{ + // Arbitrary-address-capable as either role. That bit is the most significant one of the NAME, so a + // primary always loses an address claim contest for 0xF0 against a tractor's fixed-address Tractor + // ECU, instead of taking the address from it, and then moves elsewhere until update_tecu() steps it down. + isobus::NAME name = tecuNameTemplate; + name.set_arbitrary_address_capable(true); + name.set_function_instance(functionInstance); + const std::uint8_t preferredAddress = (TecuRole::Primary == role) ? isobus::preferred_addresses::IndustryGroup2::TractorECU : SECONDARY_TECU_PREFERRED_ADDRESS; + + log("TECU") << "Claiming an address as the " << ((TecuRole::Primary == role) ? "primary" : "secondary") + << " Tractor ECU (function instance " << static_cast(functionInstance) << ", preferred address " + << format_hex_address(preferredAddress) << ")" << std::endl; + tecuCF = isobus::CANNetworkManager::CANNetwork.create_internal_control_function(name, 0, preferredAddress); + tecuRole = role; + tecuFunctionInstance = functionInstance; + tecuClaimStartedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = 0; + tecuInterfacesReady = false; + tecuPlanSettled = (TecuRole::Primary == role); // The primary sends everything; nothing to settle + tecuPlan = TecuMessagePlan{}; +} + +void Application::teardown_tecu() +{ + // Interfaces first: their destructors unregister from the control function's PGN request protocol, + // which deactivate_control_function() releases + tractorFacilities.reset(); + speedMessagesInterface.reset(); + nmea2000MessageInterface.reset(); + tecuFunctionalities.reset(); + if (tecuCF) + { + isobus::CANNetworkManager::CANNetwork.deactivate_control_function(tecuCF); + tecuCF.reset(); + } + tecuInterfacesReady = false; + tecuPlanSettled = false; + tecuPlan = TecuMessagePlan{}; + tractorFacilitiesSentOnPowerUp = false; +} + +void Application::create_speed_messages_interface(const TecuMessagePlan &plan) +{ + // The stack fixes which speed messages are sent when the interface is constructed, so a changed plan + // needs a new one. Carry the current values over so implements never see a zero speed in between. + auto previous = std::move(speedMessagesInterface); + speedMessagesInterface = std::make_unique(tecuCF, + plan.sends(TecuMessage::GroundBasedSpeed), + plan.sends(TecuMessage::WheelBasedSpeed), + plan.sends(TecuMessage::MachineSelectedSpeed), + false); + speedMessagesInterface->initialize(); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable); + speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable); + speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed); + + if (previous) + { + auto &next = *speedMessagesInterface; + next.groundBasedSpeedTransmitData.set_machine_speed(previous->groundBasedSpeedTransmitData.get_machine_speed()); + next.groundBasedSpeedTransmitData.set_machine_distance(previous->groundBasedSpeedTransmitData.get_machine_distance()); + next.groundBasedSpeedTransmitData.set_machine_direction_of_travel(previous->groundBasedSpeedTransmitData.get_machine_direction_of_travel()); + next.wheelBasedSpeedTransmitData.set_machine_speed(previous->wheelBasedSpeedTransmitData.get_machine_speed()); + next.wheelBasedSpeedTransmitData.set_machine_distance(previous->wheelBasedSpeedTransmitData.get_machine_distance()); + next.wheelBasedSpeedTransmitData.set_machine_direction_of_travel(previous->wheelBasedSpeedTransmitData.get_machine_direction_of_travel()); + next.machineSelectedSpeedTransmitData.set_machine_speed(previous->machineSelectedSpeedTransmitData.get_machine_speed()); + next.machineSelectedSpeedTransmitData.set_machine_distance(previous->machineSelectedSpeedTransmitData.get_machine_distance()); + next.machineSelectedSpeedTransmitData.set_machine_direction_of_travel(previous->machineSelectedSpeedTransmitData.get_machine_direction_of_travel()); + } + if (tractorFacilities) + { + tractorFacilities->set_speed_messages_interface(speedMessagesInterface.get()); + } +} + +void Application::apply_tecu_plan(const TecuMessagePlan &plan) +{ + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + if (plan.send[i] != tecuPlan.send[i]) { - log("Info") << "Tractor ECU disabled in settings, skipping ECU initialization." << std::endl; + const auto message = static_cast(i); + log("TECU") << (plan.send[i] ? "Sending " : "Not sending ") << tecu_message_name(message) + << (plan.send[i] ? ": no other Tractor ECU provides it" : ": another Tractor ECU provides it") << std::endl; } - else + } + + const bool speedMessagesChanged = (plan.sends(TecuMessage::GroundBasedSpeed) != tecuPlan.sends(TecuMessage::GroundBasedSpeed)) || + (plan.sends(TecuMessage::WheelBasedSpeed) != tecuPlan.sends(TecuMessage::WheelBasedSpeed)) || + (plan.sends(TecuMessage::MachineSelectedSpeed) != tecuPlan.sends(TecuMessage::MachineSelectedSpeed)); + if (speedMessagesChanged) + { + create_speed_messages_interface(plan); + } + if (nmea2000MessageInterface) + { + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(settings->is_nmea_send_enabled() && plan.sends(TecuMessage::CogSogRapidUpdate)); + } + tecuPlan = plan; + + if (tractorFacilities) + { + tractorFacilities->set_provided_facilities(facilities_for_plan(plan)); + tractorFacilities->set_time_date_active(plan.sends(TecuMessage::TimeDate)); + if (tractorFacilitiesSentOnPowerUp) + { + // Implements that cached our earlier response learn what changed + tractorFacilities->send_facilities_response(); + } + } +} + +void Application::on_aog_field_opened(const std::string &how) +{ + if (!settings->is_section_control_on_field_open()) + { + return; + } + if ((0 != lastFieldOpenAppliedMs) && !isobus::SystemTiming::time_expired_ms(lastFieldOpenAppliedMs, FIELD_OPEN_DEDUPLICATE_MS)) + { + return; + } + lastFieldOpenAppliedMs = isobus::SystemTiming::get_timestamp_ms(); + if (0 == lastFieldOpenAppliedMs) + { + lastFieldOpenAppliedMs = 1; // 0 means "never" + } + + // Saves pressing AOG's SC button after every field change; same effect as PGN 241 "enabled" + log("TC") << "AOG " << how << ": switching section control on, as if AOG's SC button was pressed" << std::endl; + tcServer->update_section_control_enabled(true); +} + +void Application::update_tecu() +{ + if (TecuRole::None == tecuRole) + { + return; + } + + auto step_down = [this](const std::string &reason) { + // ISO 11783-9 4.4.2.1: the tractor's own Tractor ECU is the primary; we become a secondary, + // with a function instance above every Tractor ECU on the bus + log("TECU") << reason << ": stepping down to a secondary Tractor ECU that only adds what it lacks" << std::endl; + send_hardware_message("Tractor ECU found: AOG adds only missing data", 10, HW_MSG_INFO); + const auto all = otherTecus.refresh(ALL_FUNCTION_INSTANCES); + const int instance = all.empty() ? 1 : std::min(all.back().functionInstance + 1, ALL_FUNCTION_INSTANCES); + teardown_tecu(); + start_tecu_claim(TecuRole::Secondary, static_cast(instance)); + }; + + // A primary claim that did not end on 0xF0 lost it to another device: do not start sending from there + if ((TecuRole::Primary == tecuRole) && tecuCF && tecuCF->get_address_valid() && + (isobus::preferred_addresses::IndustryGroup2::TractorECU != tecuCF->get_address())) + { + step_down("Address 0xF0 is taken"); + return; + } + + // A claim started at runtime: finish it without blocking the main loop + if (!tecuInterfacesReady) + { + if (tecuCF && tecuCF->get_address_valid()) { - log("Warning") << "TECU Control Function not available, Speed/NMEA interfaces not created" << std::endl; + if (0 == tecuClaimedMs) + { + tecuClaimedMs = isobus::SystemTiming::get_timestamp_ms(); + tecuClaimedMs = (0 == tecuClaimedMs) ? 1 : tecuClaimedMs; + log("TECU") << "Claimed address " << format_hex_address(tecuCF->get_address()) << std::endl; + } + if (isobus::SystemTiming::time_expired_ms(tecuClaimedMs, 250)) // J1939-81 250 ms rule + { + setup_tecu_interfaces(); + } + } + else if (isobus::SystemTiming::time_expired_ms(tecuClaimStartedMs, TECU_CLAIM_TIMEOUT_MS)) + { + log("Warning") << "TECU failed to claim an address. TECU functionality will be disabled." << std::endl; + tecuAddressClaimFailed = true; + teardown_tecu(); + tecuRole = TecuRole::None; + } + return; + } + + if (!isobus::SystemTiming::time_expired_ms(lastTecuCheckMs, TECU_CHECK_INTERVAL_MS)) + { + return; + } + lastTecuCheckMs = isobus::SystemTiming::get_timestamp_ms(); + + // Only Tractor ECUs with a lower function instance count (ISO 11783-9 4.4.2.1 c). For the primary + // (instance 0) that is another instance 0: the tractor's own; a secondary appearing is no reason to step down. + const std::uint8_t deferTo = (TecuRole::Primary == tecuRole) ? 0 : static_cast(tecuFunctionInstance - 1); + const auto others = otherTecus.refresh(deferTo); + std::set addresses; + for (const auto &other : others) + { + addresses.insert(other.address); + if (0 == knownOtherTecuAddresses.count(other.address)) + { + log("TECU") << "Another Tractor ECU appeared at " << format_hex_address(other.address) << " (function instance " + << static_cast(other.functionInstance) << ", manufacturer " << other.manufacturerCode << ")" << std::endl; + } + } + for (const auto address : knownOtherTecuAddresses) + { + if (0 == addresses.count(address)) + { + log("TECU") << "The Tractor ECU at " << format_hex_address(address) << " left the bus" << std::endl; + } + } + knownOtherTecuAddresses = addresses; + + if (TecuRole::Primary == tecuRole) + { + if (!others.empty()) + { + step_down("A Tractor ECU is on the bus"); + } + else if (!tecuCF->get_address_valid()) + { + step_down("Lost address 0xF0"); + } + return; + } + + // Secondary: ask each other TECU what it provides (ISO 11783-9 4.4.2.1 d) + for (const auto &other : others) + { + if (!otherTecus.facilities_requested(other.address) && + isobus::ParameterGroupNumberRequestProtocol::request_parameter_group_number(TractorFacilities::PGN_TRACTOR_FACILITIES, tecuCF, other.controlFunction)) + { + otherTecus.mark_facilities_requested(other.address); + log("TECU") << "Requested tractor facilities (PGN 65033) from " << format_hex_address(other.address) << std::endl; + } + } + + if (!tecuPlanSettled) + { + const std::uint32_t elapsed = isobus::SystemTiming::get_time_elapsed_ms(tecuInterfacesReadyMs); + const bool answered = otherTecus.all_facilities_received() && (elapsed >= TECU_PLAN_MIN_SETTLE_MS); + if (!answered && (elapsed < TECU_PLAN_SETTLE_MS)) + { + return; } + tecuPlanSettled = true; + log("TECU") << (answered ? "The other Tractor ECUs reported their facilities" : "Not every Tractor ECU answered PGN 65033; going by what they send") + << std::endl; + } + + const TecuMessagePlan plan = plan_tecu_messages(false, otherTecus.advertised_facilities(), otherTecus.sent_by_others()); + if (plan != tecuPlan) + { + apply_tecu_plan(plan); } } @@ -501,6 +974,8 @@ void Application::setup_udp_connections() sectionStates.push_back(data[j] & (1 << i)); } } + // Older AOG versions do not send the field name, but PGN 229 only flows while a field is open + const bool resumed = (0 == lastSectionDemandMs) || isobus::SystemTiming::time_expired_ms(lastSectionDemandMs, SECTION_DEMAND_TIMEOUT_MS); lastSectionDemandMs = lastAogPacketMs; if (sectionStates != sectionDemand) { @@ -508,6 +983,10 @@ void Application::setup_udp_connections() sectionDemandChangesSinceLog++; } tcServer->update_section_states(sectionStates); + if (resumed) + { + on_aog_field_opened("started sending section states (PGN 229)"); + } } else if (pgn == 0xF1 && !data.empty()) // 241 - Section Control { @@ -515,6 +994,85 @@ void Application::setup_udp_connections() std::uint8_t sectionControlState = data[0]; log() << "Received request from AOG to change section control state to " << (sectionControlState == 1 ? "enabled" : "disabled") << std::endl; tcServer->update_section_control_enabled(sectionControlState == 1); + // Track control is separate from section control, even though the same AOG Auto command drives both. + tcServer->update_track_control_enabled(sectionControlState == 1); + } + else if (pgn == 0xEF) // 239 - Machine Data + { + // Not parsed; only counts as AOG liveness. + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + } + else if (pgn == 0xF3) // 243 - Field Name + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // The whole payload IS the UTF-8 field name — no length prefix, no offset. + // Confirmed against a live packet: a documented "length byte at offset 4, + // name at offset 5+" layout does not match what AOG actually sends — the + // payload was exactly N raw UTF-8 name bytes, nothing else. An empty + // payload means the field is closed. + if (data.empty()) + { + if (hasActiveField) + { + log("Field") << "Field closed: " << currentFieldName << std::endl; + } + currentFieldName.clear(); + hasActiveField = false; + // Invalidate any in-flight track context immediately — broadcasting + // DDI 508 without a field to scope it to would defeat the point of the + // field index folded into it below. + currentTrackContext.valid = false; + } + else + { + constexpr std::size_t MAX_FIELD_NAME_BYTES = 248; + std::size_t nameLength = data.size(); + if (nameLength > MAX_FIELD_NAME_BYTES) + { + log("Field") << "PGN 0xF3 name of " << nameLength << " bytes exceeds the documented " + << MAX_FIELD_NAME_BYTES << "-byte max; truncating." << std::endl; + nameLength = MAX_FIELD_NAME_BYTES; + } + + std::string fieldName(reinterpret_cast(data.data()), nameLength); + if (fieldName != currentFieldName || !hasActiveField) + { + currentFieldName = fieldName; + currentFieldIndex = fieldRegistry.get_or_assign_index(fieldName); + hasActiveField = true; + log("Field") << "Field opened: " << currentFieldName << " (index " << currentFieldIndex << ")" << std::endl; + } + on_aog_field_opened("opened field '" + fieldName + "'"); + } + } + else if (pgn == 0xF4) // 244 - Guidance Track Context + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // Always update currentTrackContext: when AOG sends valid=false + // (guidance off / no active track), the context must be invalidated + // so the TC stops broadcasting stale track data. + // Note: parse() handles short-payload validation internally. + currentTrackContext = trackProvider.parse(data); + + // AOG's own guidance reference ID (see GuidanceTrackProvider) is only unique + // within whichever field AOG currently has open — fold in the field's own + // persistent index (upper 16 bits) so DDI 508 is unique across fields too. + // Without an active field, there's nothing to scope the ID to — don't send it. + if (currentTrackContext.valid) + { + if (hasActiveField) + { + currentTrackContext.guidanceReferenceLineId = + (static_cast(currentFieldIndex) << 16) | + (currentTrackContext.guidanceReferenceLineId & 0xFFFFu); + } + else + { + currentTrackContext.valid = false; + } + } } else if (pgn == 0xF2 && data.size() >= 6) // Process Data { @@ -631,10 +1189,21 @@ bool Application::update() udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); + update_tecu(); - tcServer->request_measurement_commands(); - warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); - tcServer->update(); + if (is_tc_quiet()) + { + // Not calling update() also holds the stack's own TC Status; messages wait in its queue + lastTcServerUpdateCallMs = 0; + tcServer->clear_stale_clients(); + } + else + { + tcServer->request_measurement_commands(); + warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200); + tcServer->update(); + recover_stale_clients(); + } if (tcFunctionalities) tcFunctionalities->update(); if (tecuFunctionalities) @@ -646,6 +1215,20 @@ bool Application::update() } if (nmea2000MessageInterface) nmea2000MessageInterface->update(); + + // Transmit PGN 65033 once on power-up (ISO 11783-7 B.24.3 repetition + // rate: "on power-up, and then on request"). + // The send fails until the TECU has claimed an address (or if the transmit queue rejects it), + // so only stop once it went out and retry once a second until then. + static std::uint32_t lastFacilitiesAttemptMs = 0; + // A secondary TECU waits until it knows what the other Tractor ECUs provide. + if (!tractorFacilitiesSentOnPowerUp && tractorFacilities && tecuPlanSettled && + isobus::SystemTiming::time_expired_ms(lastFacilitiesAttemptMs, 1000)) + { + lastFacilitiesAttemptMs = isobus::SystemTiming::get_timestamp_ms(); + tractorFacilitiesSentOnPowerUp = tractorFacilities->send_facilities_response(); + } + if (vtClient) update_vt_client(); @@ -674,6 +1257,13 @@ bool Application::update() lastConflictCheck = isobus::SystemTiming::get_timestamp_ms(); } + static std::uint32_t lastGlobalRtsReportMs = 0; + if (isobus::SystemTiming::time_expired_ms(lastGlobalRtsReportMs, 60000)) + { + report_global_rts_counts(); + lastGlobalRtsReportMs = isobus::SystemTiming::get_timestamp_ms(); + } + // Diff active implement clients once per second so disconnect messages retain prior metadata. static std::uint32_t lastImplementScanMs = 0; if (isobus::SystemTiming::time_expired_ms(lastImplementScanMs, 1000)) @@ -719,7 +1309,7 @@ bool Application::update() // This is deferred from setup_control_functions() until UDP is ready. if (tecuAddressClaimFailed) { - send_hardware_message("TECU failed to claim address " + std::to_string(static_cast(isobus::preferred_addresses::IndustryGroup2::TractorECU)), 10, HW_MSG_ALERT); + send_hardware_message("TECU failed to claim an address", 10, HW_MSG_ALERT); tecuAddressClaimFailed = false; } @@ -769,7 +1359,8 @@ bool Application::update() } // Send J1939 PGN 65256 every 100ms (0.1 seconds) - if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF && tecuCF->get_address_valid()) + if (isobus::SystemTiming::time_expired_ms(lastJ1939SpeedTransmit, 100) && tecuCF && tecuCF->get_address_valid() && + tecuPlan.sends(TecuMessage::VehicleDirectionSpeed)) { std::uint16_t speed_j1939 = (static_cast(std::abs(lastSpeedValue)) * 576u + 312u) / 625u; // mm/s -> (km/h)*256 std::array j1939_speed_data = { @@ -803,7 +1394,11 @@ bool Application::update() lastLateWarningMs = isobus::SystemTiming::get_timestamp_ms(); } } - if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) + if (is_tc_quiet()) + { + lastTCStatusTransmit = 0; // Send right away when the quiet period ends, without a "late" warning + } + else if (isobus::SystemTiming::time_expired_ms(lastTCStatusTransmit, 2000) && tcCF && tcCF->get_address_valid()) { static bool firstStatusSent = false; send_task_controller_status_message(); @@ -834,9 +1429,33 @@ bool Application::update() lastGnssSendMs = isobus::SystemTiming::get_timestamp_ms(); } + // Send guidance track data (DDI 507-511, 513) to implements every 250 ms + static std::uint32_t lastTrackSendMs = 0; + if (tcServer && isobus::SystemTiming::time_expired_ms(lastTrackSendMs, 250)) + { + // AOG only sends PGN 0xF4 when the guidance track actually changes (no heartbeat) — + // long gaps between packets are the normal state while driving straight, not staleness. + // Only clear the context on a real AOG disconnect (edge-triggered, not every tick). + const bool aogConnectedNow = is_aog_connected(); + if (!aogConnectedNow && aogWasConnectedForTrack) + { + currentTrackContext.valid = false; + trackProvider.reset(); // Treat next packet as fresh start after the gap + } + aogWasConnectedForTrack = aogConnectedNow; + + tcServer->send_guidance_track_data(currentTrackContext, lastXteValue); + lastTrackSendMs = isobus::SystemTiming::get_timestamp_ms(); + } + return true; } +bool Application::is_aog_connected() const +{ + return (lastAogPacketMs != 0) && !isobus::SystemTiming::time_expired_ms(lastAogPacketMs, AOG_CONNECTION_TIMEOUT_MS); +} + void Application::send_hardware_message(const std::string &text, std::uint8_t duration, std::uint8_t color) { if (!text.empty()) @@ -947,6 +1566,73 @@ Application::ImplementDetails Application::derive_implement_details(ClientState return details; } +void Application::start_tc_quiet_period(const std::string &reason) +{ + tcQuietUntilMs = isobus::SystemTiming::get_timestamp_ms() + TC_QUIET_PERIOD_MS; + if (0 == tcQuietUntilMs) + { + tcQuietUntilMs = 1; // 0 means "not quiet" + } + log("TC") << "Holding TC Status for " << TC_QUIET_PERIOD_MS << " ms (" << reason + << ") so connected implements drop their connection and reconnect" << std::endl; +} + +bool Application::is_tc_quiet() +{ + if (0 == tcQuietUntilMs) + { + return false; + } + // Signed difference so the comparison survives timestamp wrap-around + if (static_cast(isobus::SystemTiming::get_timestamp_ms() - tcQuietUntilMs) < 0) + { + return true; + } + tcQuietUntilMs = 0; + log("TC") << "Quiet period over, sending TC Status again" << std::endl; + return false; +} + +void Application::recover_stale_clients() +{ + for (const auto &client : tcServer->take_stale_clients()) + { + const std::uint64_t name = client->get_NAME().get_full_name(); + std::ostringstream clientText; + clientText << "0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(client->get_address()) + << " (NAME " << std::dec << name << ")"; + + if ((0 != lastStaleClientRecoveryMs) && + !isobus::SystemTiming::time_expired_ms(lastStaleClientRecoveryMs, STALE_CLIENT_RECOVERY_COOLDOWN_MS)) + { + continue; // Give the clients of the last quiet period time to reconnect + } + + auto &recoveries = staleClientRecoveries[name]; + if (recoveries >= MAX_STALE_CLIENT_RECOVERIES) + { + if (MAX_STALE_CLIENT_RECOVERIES == recoveries) + { + log("TC") << "Client " << clientText.str() << " is still talking to us without being connected after " + << static_cast(MAX_STALE_CLIENT_RECOVERIES) + << " quiet periods; giving up. Power-cycle the implement to reconnect it." << std::endl; + send_hardware_message("Implement not reconnecting: power-cycle it", 20, HW_MSG_ALERT); + recoveries++; // Log once + } + continue; + } + + recoveries++; + lastStaleClientRecoveryMs = isobus::SystemTiming::get_timestamp_ms(); + log("TC") << "Client " << clientText.str() << " is talking to us as if connected, but it is not one of our clients " + << "(it missed a TC restart or timeout). Reconnecting it, attempt " << static_cast(recoveries) + << " of " << static_cast(MAX_STALE_CLIENT_RECOVERIES) << "." << std::endl; + send_hardware_message("Reconnecting implement", 5, HW_MSG_INFO); + start_tc_quiet_period("stale client " + clientText.str()); + break; // One quiet period reconnects every client + } +} + std::string Application::describe_section_demand(const std::vector &demand, std::uint8_t numberOfSections) { // Sections that are on, as ranges: "4 on: 3-6" @@ -1195,7 +1881,7 @@ void Application::setup_vt_client() saved = settings->set_nmea_send_enabled(enabled); if (saved) { - nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(enabled); + nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(enabled && tecuPlan.sends(TecuMessage::CogSogRapidUpdate)); } } else @@ -1396,7 +2082,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); @@ -1472,6 +2158,42 @@ void Application::update_vt_status_strings(bool aogConnected) mainImplementStatus << "Name " << implementDisplayName << '\n' << "Sections " << totalSections << '\n' << "Section control " << sectionControl; + + // Live guidance track from AOG, plus the track control levels the implement reports (DDI 505) + { + std::string trackState = "OFF"; + if (!aogConnected) + { + trackState = "n/a"; + } + else if (currentTrackContext.valid) + { + trackState = "ref:" + std::to_string(currentTrackContext.guidanceReferenceLineId) + + " track:" + std::to_string(currentTrackContext.actualTrackNumber); + } + + int implementTrackLevels = 0; + for (const auto &client : clients) + { + if (client.second.get_supported_track_control_levels() != 0) + { + implementTrackLevels = client.second.get_supported_track_control_levels(); + break; + } + } + std::string trackLevels; + if (implementTrackLevels & static_cast(TrackControlLevel::Level1)) + trackLevels += "L1 "; + if (implementTrackLevels & static_cast(TrackControlLevel::Level2)) + trackLevels += "L2 "; + if (implementTrackLevels & static_cast(TrackControlLevel::Level3)) + trackLevels += "L3 "; + if (trackLevels.empty()) + trackLevels = "NONE"; + + mainImplementStatus << "\nTrack state " << trackState + << "\nTrack levels " << trackLevels; + } send_vt_string_if_changed(VTSectionsFromAOGS, mainImplementStatus.str()); std::ostringstream distanceText; @@ -1497,8 +2219,8 @@ void Application::update_vt_status_strings(bool aogConnected) ConfigNmeaSendLabel, // pool length 14 nmea2000MessageInterface ? (std::string("NMEA Send: ") + (settings->is_nmea_send_enabled() ? "ON" : "OFF")) : "NMEA Send: N/A"); send_vt_string_if_changed( - ConfigTecuLabel, - std::string("TECU: ") + (settings->is_tecu_enabled() ? "ON" : "OFF") + " (restart req)"); + ConfigTecuLabel, // pool length 23. Off: the tractor's TECU leads and ours only fills its gaps + settings->is_tecu_enabled() ? "TECU: AOG (restart req)" : "TECU: tractor (restart)"); std::ostringstream networkRows; std::uint8_t displayedCFs = 0; diff --git a/src/field_registry.cpp b/src/field_registry.cpp new file mode 100644 index 0000000..a0f8b76 --- /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()); + } + + log("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()) + { + log("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) + { + log("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); + log("FieldRegistry") << "Assigned index " << assigned << " to field '" << fieldName << "'" << std::endl; + return assigned; +} diff --git a/src/guidance_track_context.cpp b/src/guidance_track_context.cpp new file mode 100644 index 0000000..0aa5435 --- /dev/null +++ b/src/guidance_track_context.cpp @@ -0,0 +1,104 @@ +/** + * @file guidance_track_context.cpp + * @brief Implementation of GuidanceTrackProvider, see guidance_track_context.hpp. + */ + +#include "guidance_track_context.hpp" +#include "logging_utils.hpp" + +#include + +namespace +{ + /// @brief Decode a little-endian uint16 from a 2-byte span starting at offset. + std::uint16_t decode_le_u16(std::span data, std::size_t offset) + { + return static_cast(data[offset]) | + (static_cast(data[offset + 1]) << 8); + } + + /// @brief Decode a little-endian, signed int16 from a 2-byte span starting at offset. + std::int16_t decode_le_i16(std::span data, std::size_t offset) + { + return static_cast(decode_le_u16(data, offset)); + } +} // namespace + +GuidanceTrackContext GuidanceTrackProvider::parse(std::span data) +{ + GuidanceTrackContext ctx; + + if (data.size() < MIN_PAYLOAD_SIZE) + { + log("TRACK") << "[real] PGN 0xF4 too short (len=" + << data.size() << ")" << std::endl; + return ctx; + } + + // Parse fields + std::uint8_t sequence = data[0]; + std::uint8_t flags = data[1]; + bool isValid = (flags & 0x01) != 0; + bool headingSameWay = (flags & 0x02) != 0; + bool curveMode = (flags & 0x04) != 0; + + std::uint16_t refId = decode_le_u16(data, 2); + + // AOG's raw track-number wire convention is one pass off from what a tramline + // implement's own on-board phase computation (e.g. "which of N passes is this") + // expects, for both left and right passes. Confirmed via field testing: with the + // raw value relayed as-is, the sprayer's ON/OFF tram state was consistently one + // pass out of phase with AOG's own intended tram state; shifting every value by + // +1 (independent of sign) brought them into agreement across every pass tested. + static constexpr std::int16_t AOG_TRACK_NUMBER_OFFSET = 1; + std::int16_t currentTrack = static_cast(decode_le_i16(data, 4) + AOG_TRACK_NUMBER_OFFSET); + std::int16_t trackLeft = static_cast(decode_le_i16(data, 6) + AOG_TRACK_NUMBER_OFFSET); + std::int16_t trackRight = static_cast(decode_le_i16(data, 8) + AOG_TRACK_NUMBER_OFFSET); + + // Sequence freshness check: signed delta over the 0-255 wrap must be strictly + // positive (forward progress). Rejects both frozen/duplicate packets (delta == 0) + // and reordered/stale packets that arrived out of order (delta < 0). + const char *outcome; + + if (lastSequence_.has_value() && static_cast(sequence - *lastSequence_) <= 0) + { + outcome = "REJECTED (stale/duplicate/out-of-order sequence)"; + } + else + { + lastSequence_ = sequence; + + if (!isValid || refId == 0) + { + outcome = "guidance OFF (no active track)"; + } + else + { + ctx.guidanceReferenceLineId = refId; + ctx.actualTrackNumber = currentTrack; + ctx.trackNumberLeft = trackLeft; + ctx.trackNumberRight = trackRight; + if (data.size() >= PAYLOAD_SIZE_WITH_SWATH_WIDTH) + { + ctx.swathWidthMm = decode_le_u16(data, 10); + } + ctx.valid = true; + outcome = "ACCEPTED"; + } + } + + log("TRACK") << "[real] seq=" << static_cast(sequence) + << " flags=0x" << std::hex << static_cast(flags) << std::dec + << " (valid=" << isValid << " sameHeading=" << headingSameWay << " curve=" << curveMode << ")" + << " ref=" << refId + << " left=" << trackLeft << " actual=" << currentTrack << " right=" << trackRight + << " swath=" << ctx.swathWidthMm << "mm" + << " -> " << outcome << std::endl; + + return ctx; +} + +void GuidanceTrackProvider::reset() +{ + lastSequence_.reset(); +} diff --git a/src/logging.cpp b/src/logging.cpp index 2534664..37c679f 100644 --- a/src/logging.cpp +++ b/src/logging.cpp @@ -78,6 +78,18 @@ class CustomLogger : public isobus::CANStackLogger void sink_CAN_stack_log(CANStackLogger::LoggingLevel level, const std::string &text) override { + // Some bus devices send these continuously. The stack's text has no source address; the application + // reports them per source once per minute instead (see report_global_rts_counts in app.cpp). + if ((LoggingLevel::Warning == level) && + (text == "[TP]: Received a Request to Send (RTS) message with a global destination, ignoring")) + { + level = LoggingLevel::Debug; + if (get_log_level() > LoggingLevel::Debug) + { + return; + } + } + std::ostream &out = async_log::stream(); out << "[" << get_timestamp() << "] "; switch (level) diff --git a/src/main.cpp b/src/main.cpp index 3cf00a6..a22df3a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -104,6 +104,7 @@ enum class CANAdapter ADAPTER_INNOMAKER_USB2CAN, ADAPTER_RUSOKU_TOUCAN, ADAPTER_SYS_TEC_USB2CAN, + ADAPTER_CANDLELIGHT, ADAPTER_SOCKETCAN, }; @@ -150,7 +151,7 @@ class ArgumentProcessor std::cout << "Options:\n"; std::cout << " --help\t\tShow this help message\n"; std::cout << " --version\t\tShow the version of the application\n"; - std::cout << " --can_adapter=\tSelect the CAN driver (peak-pcan, innomaker-usb2can, rusoku-toucan, sys-tec-usb2can, socketcan)\n"; + std::cout << " --can_adapter=\tSelect the CAN driver (peak-pcan, innomaker-usb2can, rusoku-toucan, sys-tec-usb2can, candlelight, socketcan)\n"; std::cout << " --can_channel=\tSelect the CAN channel (numeric, or interface name e.g. can0 for socketcan)\n"; std::cout << " --log_level=\tSet the log level (debug, info, warning, error, critical)\n"; std::cout << " --log2file\t\tLog to file\n"; @@ -189,6 +190,8 @@ class ArgumentProcessor { "innomaker-usb2can", CANAdapter::ADAPTER_INNOMAKER_USB2CAN }, { "rusoku-toucan", CANAdapter::ADAPTER_RUSOKU_TOUCAN }, { "sys-tec-usb2can", CANAdapter::ADAPTER_SYS_TEC_USB2CAN }, + { "candlelight", CANAdapter::ADAPTER_CANDLELIGHT }, + { "gs_usb", CANAdapter::ADAPTER_CANDLELIGHT }, { "socketcan", CANAdapter::ADAPTER_SOCKETCAN }, }; @@ -269,6 +272,15 @@ static std::shared_ptr create_can_driver(const Argume return std::make_shared(static_cast(std::stoi(args.get_can_channel()))); } #endif +#if defined(ISOBUS_WINDOWSCANDLELIGHT_AVAILABLE) + case CANAdapter::ADAPTER_CANDLELIGHT: + { + // candleLight/gs_usb firmware over WinUSB. The channel picks the adapter, 1 = first one found + const std::string channel = args.get_can_channel(); + const int adapterNumber = channel.empty() ? 1 : std::stoi(channel); + return std::make_shared(static_cast(std::max(adapterNumber, 1) - 1)); + } +#endif #if defined(ISOBUS_SOCKETCAN_AVAILABLE) case CANAdapter::ADAPTER_SOCKETCAN: { diff --git a/src/settings.cpp b/src/settings.cpp index 960927a..8047930 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -70,6 +70,8 @@ bool Settings::load() nmeaSendEnabled = loadBoolean("nmeaSendEnabled", DEFAULT_NMEA_SEND_ENABLED); aogHeartbeatEnabled = loadBoolean("aogHeartbeatEnabled", DEFAULT_AOG_HEARTBEAT_ENABLED); vtEnabled = loadBoolean("vtEnabled", DEFAULT_VT_ENABLED); + sectionControlOnConnect = loadBoolean("sectionControlOnConnect", DEFAULT_SECTION_CONTROL_ON_CONNECT); + sectionControlOnFieldOpen = loadBoolean("sectionControlOnFieldOpen", DEFAULT_SECTION_CONTROL_ON_FIELD_OPEN); if (data.contains("tcVersion")) { @@ -97,6 +99,27 @@ bool Settings::load() tcVersion = DEFAULT_TC_VERSION; // Key not found, use default } + tcNumber = DEFAULT_TC_NUMBER; + if (data.contains("tcNumber")) + { + try + { + int number = data["tcNumber"].get(); + if (number >= TC_NUMBER_AUTOMATIC && number <= MAX_TC_NUMBER) + { + tcNumber = static_cast(number); + } + else + { + std::cout << "[" << get_timestamp() << "] Invalid tcNumber " << number << " (1-" << static_cast(MAX_TC_NUMBER) << ", or 0 for automatic), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + } + } + catch (const nlohmann::json::exception &e) + { + std::cout << "[" << get_timestamp() << "] Error parsing 'tcNumber': " << e.what() << std::endl; + } + } + if (data.contains("languageCode")) { try @@ -143,7 +166,10 @@ bool Settings::save() const data["nmeaSendEnabled"] = nmeaSendEnabled; data["aogHeartbeatEnabled"] = aogHeartbeatEnabled; data["vtEnabled"] = vtEnabled; + data["sectionControlOnConnect"] = sectionControlOnConnect; + data["sectionControlOnFieldOpen"] = sectionControlOnFieldOpen; data["tcVersion"] = tcVersion; + data["tcNumber"] = tcNumber; data["languageCode"] = languageCode; data["countryCode"] = countryCode; @@ -261,6 +287,28 @@ bool Settings::set_vt_enabled(bool enabled, bool save) return set_boolean(vtEnabled, enabled, save); } +bool Settings::is_section_control_on_connect() const +{ + std::scoped_lock lock(settingsMutex); + return sectionControlOnConnect; +} + +bool Settings::set_section_control_on_connect(bool enabled, bool save) +{ + return set_boolean(sectionControlOnConnect, enabled, save); +} + +bool Settings::is_section_control_on_field_open() const +{ + std::scoped_lock lock(settingsMutex); + return sectionControlOnFieldOpen; +} + +bool Settings::set_section_control_on_field_open(bool enabled, bool save) +{ + return set_boolean(sectionControlOnFieldOpen, enabled, save); +} + bool Settings::is_aog_heartbeat_enabled() const { std::scoped_lock lock(settingsMutex); @@ -297,6 +345,36 @@ bool Settings::set_tc_version(std::uint8_t version, bool save) return true; } +std::uint8_t Settings::get_tc_number() const +{ + std::scoped_lock lock(settingsMutex); + return tcNumber; +} + +bool Settings::is_tc_number_automatic() const +{ + return TC_NUMBER_AUTOMATIC == get_tc_number(); +} + +bool Settings::set_tc_number(std::uint8_t number, bool save) +{ + std::scoped_lock lock(settingsMutex); + if (number > MAX_TC_NUMBER) + { + std::cout << "[" << get_timestamp() << "] Invalid TC number " << static_cast(number) << " (1-" << static_cast(MAX_TC_NUMBER) << ", or 0 for automatic), using default " << static_cast(DEFAULT_TC_NUMBER) << std::endl; + tcNumber = DEFAULT_TC_NUMBER; + } + else + { + tcNumber = number; + } + if (save) + { + return this->save(); + } + return true; +} + std::string Settings::get_language_code() const { std::scoped_lock lock(settingsMutex); diff --git a/src/task_controller.cpp b/src/task_controller.cpp index a347816..5ed78b9 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -10,8 +10,10 @@ #include "logging_utils.hpp" #include "settings.hpp" +#include "isobus/isobus/can_network_manager.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" +#include "isobus/utility/system_timing.hpp" #include #include @@ -21,6 +23,10 @@ #include #include +/// Delay between pool activation and applying the initial section control state, so the implement +/// has settled and answered the measurement commands before it is switched to automatic. +static constexpr std::uint32_t INITIAL_SECTION_CONTROL_DELAY_MS = 2000; + // Sanitize a string for use as a filename by replacing invalid characters with underscores static std::string sanitize_filename(const std::string &input) { @@ -108,6 +114,39 @@ static void save_received_ddop(std::uint64_t clientName, const std::vector(ddi)) + { + case isobus::DataDescriptionIndex::GNSSQuality: + case isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber: + case isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID: + case isobus::DataDescriptionIndex::ActualGuidanceTrackNumber: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft: + case isobus::DataDescriptionIndex::GuidanceLineSwathWidth: + case isobus::DataDescriptionIndex::GuidanceLineDeviation: + return true; + default: + return false; + } +} + +// The DDIs used to negotiate a track control level and report the control state. +static bool is_track_control_ddi(std::uint16_t ddi) +{ + switch (static_cast(ddi)) + { + case isobus::DataDescriptionIndex::SupportedTrackControlLevels: + case isobus::DataDescriptionIndex::SetpointTrackControlLevel: + case isobus::DataDescriptionIndex::TrackControlState: + return true; + default: + return false; + } +} + void ClientState::set_number_of_sections(std::uint8_t number) { numberOfSections = number; @@ -241,6 +280,22 @@ void ClientState::set_section_control_enabled(bool state) isSectionControlEnabled = state; } +void ClientState::schedule_initial_section_control(std::uint32_t dueMs) +{ + initialSectionControlDueMs = dueMs; +} + +bool ClientState::take_due_initial_section_control(std::uint32_t nowMs) +{ + // Signed difference so the comparison survives timestamp wrap-around + if (!initialSectionControlDueMs || static_cast(nowMs - *initialSectionControlDueMs) < 0) + { + return false; + } + initialSectionControlDueMs.reset(); + return true; +} + bool ClientState::uses_per_element_control() const { return usesPerElementControl; @@ -328,6 +383,48 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool & return false; } +std::uint32_t ClientState::update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId) +{ + // A line switch can land on the same track index, so compare the reference line too. + if ((trackNumber != lastSentTrackNumber) || (referenceLineId != lastSentReferenceLineId)) + { + guidanceTrackSequenceNumber++; + lastSentTrackNumber = trackNumber; + lastSentReferenceLineId = referenceLineId; + } + return guidanceTrackSequenceNumber; +} + +int ClientState::get_supported_track_control_levels() const +{ + return supportedTrackControlLevels; +} + +void ClientState::set_supported_track_control_levels(int levels) +{ + supportedTrackControlLevels = levels; +} + +bool ClientState::is_track_control_level_sent() const +{ + return trackControlLevelSent; +} + +void ClientState::set_track_control_level_sent(bool sent) +{ + trackControlLevelSent = sent; +} + +bool ClientState::is_track_negotiation_complete() const +{ + return trackNegotiationComplete; +} + +void ClientState::set_track_negotiation_complete(bool complete) +{ + trackNegotiationComplete = complete; +} + MyTCServer::MyTCServer(std::shared_ptr internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -340,6 +437,77 @@ MyTCServer::MyTCServer(std::shared_ptr internal { } +MyTCServer::~MyTCServer() +{ + if (staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + } +} + +void MyTCServer::start_stale_client_detection() +{ + if (!staleClientDetectionStarted) + { + isobus::CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback( + static_cast(isobus::CANLibParameterGroupNumber::ProcessData), on_process_data_for_stale_check, this); + staleClientDetectionStarted = true; + } +} + +void MyTCServer::on_process_data_for_stale_check(const isobus::CANMessage &message, void *parentPointer) +{ + // Runs on the CAN thread: only collect here, the active client list is checked on the update() thread + auto server = static_cast(parentPointer); + if ((nullptr == server) || + (isobus::CAN_DATA_LENGTH != message.get_data_length()) || + (nullptr == message.get_source_control_function()) || + (server->serverControlFunction != message.get_destination_control_function())) + { + return; + } + + // A client sends these only once it considers itself connected + const auto command = static_cast(message.get_data()[0] & 0x0F); + if ((ProcessDataCommands::ClientTask == command) || + (ProcessDataCommands::Value == command) || + (ProcessDataCommands::SetValueAndAcknowledge == command)) + { + std::lock_guard lock(server->staleCandidatesMutex); + if (server->staleCandidates.end() == std::find(server->staleCandidates.begin(), server->staleCandidates.end(), message.get_source_control_function())) + { + server->staleCandidates.push_back(message.get_source_control_function()); + } + } +} + +std::vector> MyTCServer::take_stale_clients() +{ + std::vector> candidates; + { + std::lock_guard lock(staleCandidatesMutex); + candidates.swap(staleCandidates); + } + + // update() has processed the stack's queue by now, so a client that just connected is already active + std::vector> staleClients; + for (const auto &candidate : candidates) + { + if (candidate->get_address_valid() && (nullptr == get_active_client(candidate))) + { + staleClients.push_back(candidate); + } + } + return staleClients; +} + +void MyTCServer::clear_stale_clients() +{ + std::lock_guard lock(staleCandidatesMutex); + staleCandidates.clear(); +} + bool MyTCServer::activate_object_pool(std::shared_ptr partnerCF, ObjectPoolActivationError &activationError, ObjectPoolErrorCodes &objectPoolError, std::uint16_t &parentObjectIDOfFaultyObject, std::uint16_t &faultyObjectID) { std::lock_guard lock(clientsMutex); @@ -559,6 +727,11 @@ bool MyTCServer::activate_object_pool(std::shared_ptr p log() << " Section " << static_cast(i) << " -> element " << sectionElementNumbers[i] << std::endl; } } + else if (0 == numberOfSections) + { + // e.g. a tractor ECU describing hitch or GNSS geometry: nothing to control, nothing wrong + log("TC Server") << "Non-section client: the DDOP has no section elements, section control not applicable." << std::endl; + } else { log("TC Server") << "WARNING: No supported section control method detected! " @@ -714,6 +887,51 @@ bool MyTCServer::on_value_command(std::shared_ptr partn } } } + break; + + case static_cast(isobus::DataDescriptionIndex::SupportedTrackControlLevels): + { + // DDI 505 is a BITMASK: bit 0 = Level 1, bit 1 = Level 2, bit 2 = Level 3, so a value + // of 3 means Level 1 and 2 are supported (not "Level 3"). DDI 506, which the TC writes + // in answer, is an ENUM instead: 0 = no common level, 1 = Level 1, 2 = Level 2, 3 = Level 3. + log("TC") << "Implement reports supported track control levels=" << processDataValue + << " (L1=" << ((processDataValue & static_cast(TrackControlLevel::Level1)) ? "yes" : "no") + << " L2=" << ((processDataValue & static_cast(TrackControlLevel::Level2)) ? "yes" : "no") + << " L3=" << ((processDataValue & static_cast(TrackControlLevel::Level3)) ? "yes" : "no") << ")" << std::endl; + auto &state = clients[partner]; + state.set_supported_track_control_levels(processDataValue); + + // Only Level 1 is implemented, so ask for it whenever the implement supports it, even if it + // also advertises Level 2 or 3. + if (!state.is_track_control_level_sent() && + state.has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTrackControlLevel)) + { + const std::int32_t requestedLevel = (processDataValue & static_cast(TrackControlLevel::Level1)) ? 1 : 0; + send_set_value(partner, + static_cast(isobus::DataDescriptionIndex::SetpointTrackControlLevel), + state.get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointTrackControlLevel), + requestedLevel); + state.set_track_control_level_sent(true); + log("TC") << "Wrote track control level " << requestedLevel << std::endl; + } + } + break; + + case static_cast(isobus::DataDescriptionIndex::SetpointTrackControlLevel): + { + // The implement's echo of the level we wrote completes the negotiation. + const bool agreed = (processDataValue == 1); + log("TC") << "Implement confirmed track control level " << processDataValue + << (agreed ? " — negotiation complete" : " — no common level") << std::endl; + clients[partner].set_track_negotiation_complete(agreed); + } + break; + + case static_cast(isobus::DataDescriptionIndex::TrackControlState): + { + log("TC") << "Implement reports track control state=" << processDataValue << std::endl; + } + break; } return true; @@ -839,8 +1057,8 @@ void MyTCServer::request_measurement_commands() } } - // Map GNSS quality (DDI 514) so send_gnss_quality() knows which element to address. - // Mapping only: this is a value the TC pushes to the implement, not one to subscribe to. + // Map the guidance DDIs so the send_* methods know which element to address. + // Mapping only: these are values the TC pushes to the implement, not ones to subscribe to. for (std::uint32_t i = 0; i < client.second.get_pool().size(); i++) { auto object = client.second.get_pool().get_object_by_index(i); @@ -850,28 +1068,73 @@ void MyTCServer::request_measurement_commands() } auto processDataObject = std::dynamic_pointer_cast(object); - if (!processDataObject || processDataObject->get_ddi() != static_cast(isobus::DataDescriptionIndex::GNSSQuality)) + if (!processDataObject || !is_guidance_data_ddi(processDataObject->get_ddi())) { continue; } if (!processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) { - log() << "DDI " << processDataObject->get_ddi() << " (GNSS Quality) is declared but not settable, so it is not sent" << std::endl; + log() << "DDI " << processDataObject->get_ddi() << " (" << isobus::DataDictionary::get_entry(processDataObject->get_ddi()).to_string() << ") is declared but not settable, so it is not sent" << std::endl; continue; } std::uint16_t elementNumber = 0; if (find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber)) { - client.second.set_element_number_for_ddi(isobus::DataDescriptionIndex::GNSSQuality, elementNumber); - log() << "Mapped DDI " << processDataObject->get_ddi() << " (GNSS Quality) to element " + client.second.set_element_number_for_ddi(static_cast(processDataObject->get_ddi()), elementNumber); + log() << "Mapped DDI " << processDataObject->get_ddi() << " (" << isobus::DataDictionary::get_entry(processDataObject->get_ddi()).to_string() << ") to element " << elementNumber << std::endl; } } + // Map the track control DDIs. Unlike the guidance data above, the implement reports these + // (supported levels, the echo of the level we write, its control state), so subscribe to them. + for (std::uint32_t i = 0; i < client.second.get_pool().size(); i++) + { + auto object = client.second.get_pool().get_object_by_index(i); + if (!object || object->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceProcessData) + { + continue; + } + + auto processDataObject = std::dynamic_pointer_cast(object); + if (!processDataObject || !is_track_control_ddi(processDataObject->get_ddi())) + { + continue; + } + + std::uint16_t elementNumber = 0; + if (!find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber)) + { + continue; + } + + const auto ddi = processDataObject->get_ddi(); + const auto &entry = isobus::DataDictionary::get_entry(ddi); + client.second.set_element_number_for_ddi(static_cast(ddi), elementNumber); + if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) + { + send_change_threshold_measurement_command(client.first, ddi, elementNumber, 1); + log() << "Subscribed (OnChange) to DDI " << ddi << " (" << entry.to_string() << ") for element " << elementNumber << std::endl; + } + else + { + log() << "Mapped (no OnChange) DDI " << ddi << " (" << entry.to_string() << ") to element " << elementNumber << std::endl; + } + } + log() << "Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); + + // Switch the implement to automatic shortly after it connects, so AOG's SC button + // does not need pressing on every connect. + client.second.schedule_initial_section_control(isobus::SystemTiming::get_timestamp_ms() + INITIAL_SECTION_CONTROL_DELAY_MS); + } + if (client.second.take_due_initial_section_control(isobus::SystemTiming::get_timestamp_ms())) + { + log("TC") << "Applying initial section control state: " << (desiredSectionControlEnabled ? "enabled" : "disabled") << std::endl; + apply_section_control_state(client.first, client.second, desiredSectionControlEnabled); } } } @@ -925,20 +1188,29 @@ void MyTCServer::update_section_states(std::vector §ionStates) void MyTCServer::update_section_control_enabled(bool enabled) { std::lock_guard lock(clientsMutex); + desiredSectionControlEnabled = enabled; for (auto &client : clients) { - // Always update the local flag - if (client.second.is_section_control_enabled() != enabled) - { - client.second.set_section_control_enabled(enabled); - } + apply_section_control_state(client.first, client.second, enabled); + } +} - // Only send ISOBUS command to clients that support SectionControlState DDI and have sections - if (client.second.has_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState) && - client.second.get_number_of_sections() > 0) - { - send_section_control_state(client.first, enabled); - } +void MyTCServer::set_initial_section_control_enabled(bool enabled) +{ + std::lock_guard lock(clientsMutex); + desiredSectionControlEnabled = enabled; +} + +void MyTCServer::apply_section_control_state(std::shared_ptr client, ClientState &state, bool enabled) +{ + // Always update the local flag + state.set_section_control_enabled(enabled); + + // Only send ISOBUS command to clients that support SectionControlState DDI and have sections + if (state.has_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState) && + state.get_number_of_sections() > 0) + { + send_section_control_state(client, enabled); } } @@ -955,6 +1227,58 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality) } } +void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm) +{ + if (!ctx.valid) + { + return; + } + + std::lock_guard lock(clientsMutex); + for (auto &client : clients) + { + auto &state = client.second; + + auto trySend = [&](isobus::DataDescriptionIndex ddi, std::int32_t value) { + if (state.has_element_number_for_ddi(ddi)) + { + send_set_value(client.first, static_cast(ddi), state.get_element_number_for_ddi(ddi), value); + } + }; + + const std::uint32_t sequenceNumber = state.update_guidance_track_sequence(ctx.actualTrackNumber, ctx.guidanceReferenceLineId); + + // Coherent ordering per the TRACK guideline: + // 507 (sequence) -> 508 (ref line ID) -> 509 (actual track) -> 510 (right) -> 511 (left) + trySend(isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber, static_cast(sequenceNumber)); + trySend(isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID, static_cast(ctx.guidanceReferenceLineId)); + trySend(isobus::DataDescriptionIndex::ActualGuidanceTrackNumber, ctx.actualTrackNumber); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight, ctx.trackNumberRight); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft, ctx.trackNumberLeft); + if (ctx.swathWidthMm != 0) + { + trySend(isobus::DataDescriptionIndex::GuidanceLineSwathWidth, static_cast(ctx.swathWidthMm)); + } + trySend(isobus::DataDescriptionIndex::GuidanceLineDeviation, lineDeviationMm); + } +} + +void MyTCServer::update_track_control_enabled(bool enabled) +{ + std::lock_guard lock(clientsMutex); + constexpr auto DDI = isobus::DataDescriptionIndex::TrackControlState; + for (auto &client : clients) + { + // Only clients that agreed on a track control level get commanded. + if (client.second.is_track_negotiation_complete() && client.second.has_element_number_for_ddi(DDI)) + { + // DDI 515 values: 0 = manual/off, 1 = automatic/on + send_set_value(client.first, static_cast(DDI), client.second.get_element_number_for_ddi(DDI), enabled ? 1 : 0); + log("TC") << "Track control state=" << (enabled ? "On" : "Off") << std::endl; + } + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex); diff --git a/src/tecu_coexistence.cpp b/src/tecu_coexistence.cpp new file mode 100644 index 0000000..936b8e6 --- /dev/null +++ b/src/tecu_coexistence.cpp @@ -0,0 +1,293 @@ +/** + * @brief Sharing the implement bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1). + */ + +#include "tecu_coexistence.hpp" + +#include "isobus/isobus/can_NAME.hpp" +#include "isobus/isobus/can_network_manager.hpp" +#include "isobus/utility/system_timing.hpp" + +#include + +namespace +{ + constexpr std::size_t index_of(TecuMessage message) + { + return static_cast(message); + } +} + +std::uint32_t tecu_message_pgn(TecuMessage message) +{ + switch (message) + { + case TecuMessage::GroundBasedSpeed: + return 65097; + case TecuMessage::WheelBasedSpeed: + return 65096; + case TecuMessage::MachineSelectedSpeed: + return 61474; + case TecuMessage::VehicleDirectionSpeed: + return 65256; + case TecuMessage::CogSogRapidUpdate: + return 129026; + case TecuMessage::TimeDate: + return 65254; + default: + return 0; + } +} + +std::string tecu_message_name(TecuMessage message) +{ + std::string name; + switch (message) + { + case TecuMessage::GroundBasedSpeed: + name = "ground-based speed"; + break; + case TecuMessage::WheelBasedSpeed: + name = "wheel-based speed"; + break; + case TecuMessage::MachineSelectedSpeed: + name = "machine selected speed"; + break; + case TecuMessage::VehicleDirectionSpeed: + name = "vehicle direction/speed"; + break; + case TecuMessage::CogSogRapidUpdate: + name = "NMEA 2000 COG/SOG"; + break; + case TecuMessage::TimeDate: + name = "time/date"; + break; + default: + name = "unknown"; + break; + } + return name + " (PGN " + std::to_string(tecu_message_pgn(message)) + ")"; +} + +TecuMessagePlan plan_tecu_messages(bool isPrimary, + const Facilities &advertisedByOthers, + const std::array &sentByOthers) +{ + TecuMessagePlan plan; + if (isPrimary) + { + plan.send.fill(true); + return plan; + } + + std::array advertised{}; + advertised[index_of(TecuMessage::GroundBasedSpeed)] = advertisedByOthers.groundBasedSpeed; + advertised[index_of(TecuMessage::WheelBasedSpeed)] = advertisedByOthers.wheelBasedSpeed; + advertised[index_of(TecuMessage::MachineSelectedSpeed)] = advertisedByOthers.selectedSpeed; + advertised[index_of(TecuMessage::TimeDate)] = advertisedByOthers.timeDate; + // Vehicle direction/speed and COG/SOG have no facility bit: only what is seen on the bus counts + + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + plan.send[i] = !(advertised[i] || sentByOthers[i]); + } + return plan; +} + +Facilities facilities_for_plan(const TecuMessagePlan &plan) +{ + Facilities f; + f.tecuClass = 1; + + const bool ground = plan.sends(TecuMessage::GroundBasedSpeed); + f.groundBasedSpeed = ground; + f.groundBasedDistance = ground; + f.groundBasedDirection = ground; + + const bool wheel = plan.sends(TecuMessage::WheelBasedSpeed); + f.wheelBasedSpeed = wheel; + f.wheelBasedDistance = wheel; + f.wheelBasedDirection = wheel; + + f.selectedSpeed = plan.sends(TecuMessage::MachineSelectedSpeed); + return f; +} + +Facilities merge_facilities(const Facilities &a, const Facilities &b) +{ + // The encoded payload is the bitwise form of every facility, so OR it byte by byte + const auto pa = encode_facilities(a); + const auto pb = encode_facilities(b); + std::array merged{}; + for (std::size_t i = 0; i < merged.size(); i++) + { + merged[i] = static_cast(pa[i] | pb[i]); + } + Facilities result = decode_facilities(merged); + // The class field is not a bit set: keep the higher class ("not available" is 0 here) + result.tecuClass = std::max(a.tecuClass, b.tecuClass); + return result; +} + +std::uint32_t pgn_from_can_identifier(std::uint32_t identifier) +{ + const std::uint32_t pduFormat = (identifier >> 16) & 0xFF; + const std::uint32_t pduSpecific = (identifier >> 8) & 0xFF; + const std::uint32_t dataPages = (identifier >> 24) & 0x03; // Extended data page and data page + // PDU1 (PF < 240): the PS byte is a destination address, not part of the PGN + return (dataPages << 16) | (pduFormat << 8) | ((pduFormat >= 0xF0) ? pduSpecific : 0); +} + +void OtherTecuMonitor::initialize() +{ + isobus::CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback( + TractorFacilities::PGN_TRACTOR_FACILITIES, &OtherTecuMonitor::on_facilities_response, this); +} + +std::vector OtherTecuMonitor::refresh(std::uint8_t maxFunctionInstance) +{ + std::vector others; + for (const auto &cf : isobus::CANNetworkManager::CANNetwork.get_control_functions(false)) + { + if ((nullptr == cf) || !cf->get_address_valid() || + (isobus::ControlFunction::Type::Internal == cf->get_type()) || + (static_cast(isobus::NAME::Function::TractorECU) != cf->get_NAME().get_function_code()) || + (cf->get_NAME().get_function_instance() > maxFunctionInstance)) + { + continue; + } + OtherTecu other; + other.controlFunction = cf; + other.address = cf->get_address(); + other.functionInstance = cf->get_NAME().get_function_instance(); + other.manufacturerCode = cf->get_NAME().get_manufacturer_code(); + others.push_back(other); + } + std::sort(others.begin(), others.end(), [](const OtherTecu &a, const OtherTecu &b) { + return a.functionInstance < b.functionInstance; + }); + + std::set addresses; + std::map names; + for (const auto &other : others) + { + addresses.insert(other.address); + names[other.address] = other.controlFunction->get_NAME().get_full_name(); + } + + std::lock_guard lock(mutex); + for (std::size_t address = 0; address < isOtherTecuAddress.size(); address++) + { + isOtherTecuAddress[address] = (addresses.count(static_cast(address)) > 0); + } + for (auto it = facilitiesByAddress.begin(); it != facilitiesByAddress.end();) + { + // A TECU that left, or a different one now at that address, has to answer again + const auto name = names.find(it->first); + const auto previous = nameByAddress.find(it->first); + const bool same = (names.end() != name) && (nameByAddress.end() != previous) && (name->second == previous->second); + it = same ? std::next(it) : facilitiesByAddress.erase(it); + } + for (auto it = requestedAddresses.begin(); it != requestedAddresses.end();) + { + const auto name = names.find(*it); + const auto previous = nameByAddress.find(*it); + const bool same = (names.end() != name) && (nameByAddress.end() != previous) && (name->second == previous->second); + it = same ? std::next(it) : requestedAddresses.erase(it); + } + otherAddresses = addresses; + nameByAddress = names; + return others; +} + +void OtherTecuMonitor::on_frame(const isobus::CANMessageFrame &frame) +{ + if (!frame.isExtendedFrame || !isOtherTecuAddress[frame.identifier & 0xFF]) + { + return; + } + const std::uint32_t pgn = pgn_from_can_identifier(frame.identifier); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + if (tecu_message_pgn(static_cast(i)) == pgn) + { + const std::uint32_t now = isobus::SystemTiming::get_timestamp_ms(); + lastSentByOthersMs[i] = (0 == now) ? 1 : now; // 0 means "never" + } + } +} + +void OtherTecuMonitor::on_facilities_response(const isobus::CANMessage &message, void *parent) +{ + auto *self = static_cast(parent); + const auto source = message.get_source_control_function(); + const auto &data = message.get_data(); + if ((nullptr == self) || (nullptr == source) || (data.size() < 8)) + { + return; + } + const std::uint8_t address = source->get_address(); + if (!self->isOtherTecuAddress[address]) + { + return; // Our own response, or one from something that is not a TECU + } + + std::array payload{}; + std::copy(data.begin(), data.begin() + 8, payload.begin()); + if (payload[7] & 0x01) + { + // Reserved bit indicator set (ISO 11783-7 A.27.6): the newer facility bits are 1-filled, not real + payload[1] &= 0xFC; + payload[3] &= 0xF0; + payload[4] &= 0xE0; + payload[6] = 0; + payload[7] = 0; + } + + std::lock_guard lock(self->mutex); + self->facilitiesByAddress[address] = decode_facilities(payload); +} + +Facilities OtherTecuMonitor::advertised_facilities() const +{ + std::lock_guard lock(mutex); + Facilities combined; + combined.tecuClass = 0; + for (const auto &entry : facilitiesByAddress) + { + combined = merge_facilities(combined, entry.second); + } + return combined; +} + +bool OtherTecuMonitor::all_facilities_received() const +{ + std::lock_guard lock(mutex); + return std::all_of(otherAddresses.begin(), otherAddresses.end(), [this](std::uint8_t address) { + return facilitiesByAddress.count(address) > 0; + }); +} + +std::array OtherTecuMonitor::sent_by_others() const +{ + std::array sent{}; + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + const std::uint32_t last = lastSentByOthersMs[i]; + const bool isTimeDate = (static_cast(TecuMessage::TimeDate) == i); + sent[i] = (0 != last) && !isobus::SystemTiming::time_expired_ms(last, isTimeDate ? TIME_DATE_SENT_BY_OTHERS_WINDOW_MS : SENT_BY_OTHERS_WINDOW_MS); + } + return sent; +} + +bool OtherTecuMonitor::facilities_requested(std::uint8_t address) const +{ + std::lock_guard lock(mutex); + return requestedAddresses.count(address) > 0; +} + +void OtherTecuMonitor::mark_facilities_requested(std::uint8_t address) +{ + std::lock_guard lock(mutex); + requestedAddresses.insert(address); +} diff --git a/src/tractor_facilities.cpp b/src/tractor_facilities.cpp new file mode 100644 index 0000000..70390f6 --- /dev/null +++ b/src/tractor_facilities.cpp @@ -0,0 +1,579 @@ +/** + * @brief ISO 11783-7 Tractor Facilities (PGN 65033) implementation. + * + * @see ISO 11783-7:2009, B.24.3 (PGN 65033) and B.24.2 (PGN 65032). + */ + +#include "tractor_facilities.hpp" + +#include "isobus/isobus/can_network_manager.hpp" +#include "isobus/isobus/can_parameter_group_number_request_protocol.hpp" +#include "isobus/isobus/can_stack_logger.hpp" +#include "isobus/utility/system_timing.hpp" + +#include "logging_utils.hpp" +#include "settings.hpp" + +#include +#include +#include +#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; +} + +// --------------------------------------------------------------------------- +// Time/Date +// --------------------------------------------------------------------------- + +int utc_offset_minutes(const std::tm &utc, const std::tm &local) +{ + // The two clocks are at most a day apart. Across New Year tm_yday jumps, so compare the years first. + int dayDifference = local.tm_yday - utc.tm_yday; + if (local.tm_year != utc.tm_year) + { + dayDifference = (local.tm_year > utc.tm_year) ? 1 : -1; + } + return (dayDifference * 24 * 60) + ((local.tm_hour - utc.tm_hour) * 60) + (local.tm_min - utc.tm_min); +} + +std::array encode_time_date(const std::tm &utc, const std::tm &local, int milliseconds) +{ + const int offsetMinutes = utc_offset_minutes(utc, local); + const int seconds = std::min(utc.tm_sec, 59); // tm_sec is 60 during a leap second + const int quarterOfDay = utc.tm_hour / 6; + + std::array p{}; + p[0] = static_cast((seconds * 4) + (milliseconds / 250)); // 0.25 s/bit + p[1] = static_cast(utc.tm_min); + p[2] = static_cast(utc.tm_hour); + p[3] = static_cast(utc.tm_mon + 1); + p[4] = static_cast(((utc.tm_mday - 1) * 4) + quarterOfDay + 1); // 0.25 day/bit, 0 = not a day + p[5] = static_cast(utc.tm_year + 1900 - 1985); + // Both offsets carry the sign: -3:30 is -3 h and -30 min + p[6] = static_cast((offsetMinutes % 60) + 125); + p[7] = static_cast((offsetMinutes / 60) + 125); + return p; +} + +/// Read the PC clock. False if it is outside what the message can carry, e.g. a clock that was never set. +static bool read_system_time_date(std::array &payload) +{ + const auto now = std::chrono::system_clock::now(); + const std::time_t seconds = std::chrono::system_clock::to_time_t(now); + const auto milliseconds = std::chrono::duration_cast(now.time_since_epoch()).count() % 1000; + + std::tm utc{}; + std::tm local{}; +#if defined(_WIN32) + const bool converted = (0 == gmtime_s(&utc, &seconds)) && (0 == localtime_s(&local, &seconds)); +#else + const bool converted = (nullptr != gmtime_r(&seconds, &utc)) && (nullptr != localtime_r(&seconds, &local)); +#endif + const int year = utc.tm_year + 1900; + if (!converted || (year < 1985) || (year > 2235)) + { + return false; + } + payload = encode_time_date(utc, local, static_cast(milliseconds)); + return true; +} + +// --------------------------------------------------------------------------- +// TractorFacilities +// --------------------------------------------------------------------------- + +TractorFacilities::TractorFacilities( + std::shared_ptr tecuCF, + std::shared_ptr settings) : + tecuCF(std::move(tecuCF)), + settings(std::move(settings)) +{ +} + +TractorFacilities::~TractorFacilities() +{ + if (!initialized) + { + return; + } + // The request protocol calls on_pgn_request() under its own lock, so after this returns it no longer + // runs. The stack may still call on_required_facilities() once after removal (it iterates a copy of + // the global callbacks), which is harmless: that callback only logs and never touches this object. + if (auto pgnRequestProtocol = tecuCF ? tecuCF->get_pgn_request_protocol().lock() : nullptr) + { + pgnRequestProtocol->remove_pgn_request_callback(PGN_TRACTOR_FACILITIES, &TractorFacilities::on_pgn_request, this); + } + isobus::CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback( + PGN_REQUIRED_TRACTOR_FACILITIES, &TractorFacilities::on_required_facilities, this); + if (auto pgnRequestProtocol = tecuCF ? tecuCF->get_pgn_request_protocol().lock() : nullptr) + { + pgnRequestProtocol->remove_pgn_request_callback(PGN_TIME_DATE, &TractorFacilities::on_time_date_request, this); + } +} + +void TractorFacilities::set_provided_facilities(const Facilities &facilities) +{ + std::lock_guard lock(providedFacilitiesMutex); + providedFacilities = facilities; + hasProvidedFacilities = true; +} + +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 +{ + { + std::lock_guard lock(providedFacilitiesMutex); + if (hasProvidedFacilities) + { + Facilities provided = providedFacilities; + provided.timeDate = timeDateActive; + return encode_facilities(provided); + } + } + + Facilities f; + f.tecuClass = 1; // Class 1 – the only class we claim. + + // 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 while + // the TECU answers requests for it. + 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; + + if (!pgnRequestProtocol->register_pgn_request_callback(PGN_TIME_DATE, &TractorFacilities::on_time_date_request, this)) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Failed to register PGN 65254 request callback." << std::endl; + } + + initialized = true; + return true; +} + +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_time_date_request( + std::uint32_t parameterGroupNumber, + std::shared_ptr /*requestingControlFunction*/, + bool &acknowledge, + isobus::AcknowledgementType & /*acknowledgeType*/, + void *parentPointer) +{ + auto *self = static_cast(parentPointer); + std::array payload{}; + // Returning false leaves it to the Tractor ECU that does provide it: the stack ignores a global + // request and NACKs one addressed to us + if ((PGN_TIME_DATE != parameterGroupNumber) || (nullptr == self) || !self->timeDateActive || !read_system_time_date(payload)) + { + return false; + } + + acknowledge = false; // Sending the requested PGN is the response + const bool sent = isobus::CANNetworkManager::CANNetwork.send_can_message(PGN_TIME_DATE, payload.data(), payload.size(), self->tecuCF); + if (sent && !self->timeDateRequestLogged.exchange(true)) + { + std::cout << "[" << get_timestamp() << "] [TractorFacilities] Answering requests for PGN 65254 (Time/Date) from the PC clock." << std::endl; + } + return sent; +} + +bool TractorFacilities::on_pgn_request( + std::uint32_t parameterGroupNumber, + std::shared_ptr requestingControlFunction, + bool &acknowledge, + isobus::AcknowledgementType & /*acknowledgeType*/, + void *parentPointer) +{ + if (parameterGroupNumber != PGN_TRACTOR_FACILITIES || !parentPointer || !requestingControlFunction) + { + return false; + } + + auto *self = static_cast(parentPointer); + + // Sending the requested PGN *is* the response; an ACK on top would be + // redundant and confusing to some implement stacks. + acknowledge = false; + + // Log once per requester source address at info level. + std::uint8_t sa = requestingControlFunction->get_address(); + if (self->loggedRequesters.find(sa) == self->loggedRequesters.end()) + { + self->loggedRequesters.insert(sa); + + auto payload = self->build_payload(); + std::ostringstream hex; + hex << std::hex; + for (std::size_t i = 0; i < payload.size(); ++i) + { + if (i != 0) + hex << ' '; + hex << "0x" << static_cast(payload[i]); + } + std::cout << "[" << get_timestamp() << "] [TractorFacilities] PGN 65033 requested by SA " + << static_cast(sa) << ", advertising facilities: [" << hex.str() << "]" << std::endl; + } + + return self->send_facilities_response(false); +} + +void TractorFacilities::on_required_facilities( + const isobus::CANMessage &message, + void *parentPointer) +{ + if (!parentPointer) + { + return; + } + + const auto &data = message.get_data(); + if (data.size() < 8) + { + return; + } + + auto sourceCF = message.get_source_control_function(); + std::uint8_t sa = sourceCF ? sourceCF->get_address() : 0xFF; + + // Decode and log — diagnostic only, do not change our + // response based on what the implement asks for. + std::array raw{}; + for (std::size_t i = 0; i < 8 && i < data.size(); ++i) + { + raw[i] = data[i]; + } + + // A simple hex dump avoids pulling in the full Facilities decode for a + // diagnostic message. + std::ostringstream hex; + hex << std::hex; + for (std::size_t i = 0; i < raw.size(); ++i) + { + if (i != 0) + hex << ' '; + hex << "0x" << static_cast(raw[i]); + } + isobus::CANStackLogger::debug("[TractorFacilities] PGN 65032 from SA " + std::to_string(static_cast(sa)) + + ": required facilities [" + hex.str() + "]"); +} diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index a4e7540..6905e19 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -8,8 +8,12 @@ */ #include "udp_connections.hpp" +#include #include +#include #include +#include +#include "aog_packet_parser.hpp" #include "logging_utils.hpp" #if !defined(_WIN32) @@ -153,15 +157,80 @@ std::uint8_t UdpConnections::calculate_crc(std::span data) return static_cast(result & 0xFF); } +namespace +{ + /// Bytes of a datagram shown in the log when it is not (only) AOG packets + constexpr std::size_t MAX_LOGGED_DATAGRAM_BYTES = 64; + + /// @brief Hex dump with a "|" where parsing stopped, then the printable characters (others as ".") + std::string describe_datagram(std::span datagram, std::size_t stopOffset) + { + const std::size_t count = std::min(datagram.size(), MAX_LOGGED_DATAGRAM_BYTES); + std::ostringstream hex; + std::string text; + hex << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < count; i++) + { + hex << ((i == stopOffset) ? ((0 == i) ? "| " : " | ") : ((0 == i) ? "" : " ")) << std::setw(2) << static_cast(datagram[i]); + text += ((datagram[i] >= 0x20) && (datagram[i] < 0x7F)) ? static_cast(datagram[i]) : '.'; + } + if (count < datagram.size()) + { + hex << " ..."; + } + return hex.str() + " \"" + text + "\""; + } + + void log_parse_stop(const char *socketName, const udp::endpoint &sender, std::span datagram, const aog_packet::ParseResult &result) + { + if (aog_packet::ParseStop::Complete == result.stop) + { + return; + } + + const std::size_t dropped = datagram.size() - result.offset; + log() << ((aog_packet::ParseStop::UnknownStart == result.stop) ? "Unknown start of message" : "Truncated AOG packet") + << " from " << sender.address().to_string() << ":" << sender.port() + << " (" << socketName << ", dropped " << dropped << " of " << datagram.size() << " bytes): " + << describe_datagram(datagram, result.offset) << std::endl; + } +} + +void UdpConnections::log_unknown_start(const udp::endpoint &sender, std::span data) +{ + // Both sockets listen on port 8888 (one on the AOG subnet address, one on all interfaces), so a + // broadcast packet usually arrives on both. Only log it once. + static constexpr std::size_t MAX_LOGGED_BYTES = 16; + static constexpr auto DUPLICATE_WINDOW = std::chrono::milliseconds(50); + + std::ostringstream signature; + signature << sender.address().to_string() << ":" << sender.port() << " " << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < std::min(data.size(), MAX_LOGGED_BYTES); i++) + { + signature << (i ? " " : "") << std::setw(2) << static_cast(data[i]); + } + if (data.size() > MAX_LOGGED_BYTES) + { + signature << " ..."; + } + + const auto now = std::chrono::steady_clock::now(); + if ((signature.str() == lastUnknownStartSignature) && (now - lastUnknownStartTime < DUPLICATE_WINDOW)) + { + return; + } + lastUnknownStartSignature = signature.str(); + lastUnknownStartTime = now; + log() << "Unknown start of message from " << lastUnknownStartSignature << " (" << std::dec << data.size() << " bytes)" << std::endl; +} + void UdpConnections::handle_incoming_packets() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -169,51 +238,21 @@ void UdpConnections::handle_incoming_packets() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + if (packetCallback) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (packetCallback) - { - packetCallback(src, pgn, { rxBuffer.data() + index, len }); - } - index += len + 1; - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; - } - - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; + packetCallback(src, pgn, data); } + }); + if (aog_packet::ParseStop::UnknownStart == result.stop) + { + // Both sockets get a broadcast: log_unknown_start() drops the second copy + log_unknown_start(sender_endpoint, datagram.subspan(result.offset)); + } + else + { + log_parse_stop("AOG", sender_endpoint, datagram, result); } } else @@ -224,13 +263,11 @@ void UdpConnections::handle_incoming_packets() void UdpConnections::handle_address_detection() { - static std::array rxBuffer; - static std::size_t rxIndex = 0; + static std::array rxBuffer; - // Peek to see if we have any data boost::system::error_code error_code; udp::endpoint sender_endpoint; - size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer.data() + rxIndex, sizeof(rxBuffer) - rxIndex), sender_endpoint, 0, error_code); + size_t bytesReceived = udpConnectionAddressDetection.receive_from(boost::asio::buffer(rxBuffer), sender_endpoint, 0, error_code); if (error_code == boost::asio::error::would_block) { @@ -238,70 +275,36 @@ void UdpConnections::handle_address_detection() } else if (!error_code) { - rxIndex += bytesReceived; - std::uint8_t index = 0; - - while (rxIndex >= 8) - { - std::uint16_t start = (rxBuffer[index++] << 8) | rxBuffer[index++]; - if (start == PACKET_START) + std::span datagram(rxBuffer.data(), bytesReceived); + auto result = aog_packet::parse_datagram(datagram, [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + // 127 is source AgIO, 201 is subnet; data is 0xC9 0xC9 IP0 IP1 IP2 + if ((0x7F != src) || (0xC9 != pgn) || (5 != data.size()) || (0xC9 != data[0]) || (0xC9 != data[1])) { - std::uint8_t src = rxBuffer[index++]; - std::uint8_t pgn = rxBuffer[index++]; - std::uint8_t len = rxBuffer[index++]; - std::uint8_t crc = rxBuffer[index + len]; - - // Check CRC (skip start of packet, but include source, PGN, length and data) - // std::uint8_t crcCalc = calculate_crc({ rxBuffer.data() + index - 3, static_cast(len) + 3 }); - // if (crc != crcCalc) - // { - // std::cout << "CRC mismatch (for PGN " << static_cast(pgn) << "?), expected " << static_cast(crc) << " but got " << static_cast(crcCalc) << std::endl; - // rxIndex = 0; - // break; - // } - - if (src == 0x7F && pgn == 0xC9 && len == 5 // 127 is source, AGIO, 201 is subnet - && rxBuffer[index++] == 0xC9 && rxBuffer[index++] == 0xC9) - { - // 7-8-9 is IP0,IP1,IP2 - settings->set_subnet({ rxBuffer[index++], rxBuffer[index++], rxBuffer[index++] }); - - std::cout << "Subnet from AOG: "; - std::cout << int(settings->get_subnet()[0]) << "."; - std::cout << int(settings->get_subnet()[1]) << "."; - std::cout << int(settings->get_subnet()[2]); - std::cout << " rebinding UPD connection " << std::endl; - udpConnection.close(); - udpConnection.open(udp::v4()); - udpConnection.set_option(boost::asio::socket_base::broadcast(true)); - udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); - udpConnection.non_blocking(true); - udpConnection.bind(get_local_endpoint()); - - index += len - 4; - } - else - { - index += len + 1; - } - } - else - { - // Unknown start of message, reset buffer - log() << "Unknown start of message: 0x" << std::hex << start << std::dec << std::endl; - rxIndex = 0; + return; } - // Move any remaining data to the front of the buffer - if (index < rxIndex) - { - std::memmove(rxBuffer.data(), rxBuffer.data() + index, rxIndex - index); - rxIndex -= index; - } - else - { - rxIndex = 0; - } + settings->set_subnet({ data[2], data[3], data[4] }); + + std::cout << "Subnet from AOG: "; + std::cout << int(settings->get_subnet()[0]) << "."; + std::cout << int(settings->get_subnet()[1]) << "."; + std::cout << int(settings->get_subnet()[2]); + std::cout << " rebinding UPD connection " << std::endl; + udpConnection.close(); + udpConnection.open(udp::v4()); + udpConnection.set_option(boost::asio::socket_base::broadcast(true)); + udpConnection.set_option(boost::asio::socket_base::reuse_address(true)); + udpConnection.non_blocking(true); + udpConnection.bind(get_local_endpoint()); + }); + if (aog_packet::ParseStop::UnknownStart == result.stop) + { + // Both sockets get a broadcast: log_unknown_start() drops the second copy + log_unknown_start(sender_endpoint, datagram.subspan(result.offset)); + } + else + { + log_parse_stop("address detection", sender_endpoint, datagram, result); } } else diff --git a/tools/test_aog_packet_parser.cpp b/tools/test_aog_packet_parser.cpp new file mode 100644 index 0000000..d25db51 --- /dev/null +++ b/tools/test_aog_packet_parser.cpp @@ -0,0 +1,155 @@ +/** + * @brief Unit tests for aog_packet::parse_datagram, which splits an AgIO/AOG UDP datagram into packets. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "aog_packet_parser.hpp" + +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +struct Packet +{ + std::uint8_t src; + std::uint8_t pgn; + std::vector data; +}; + +static std::vector make_packet(std::uint8_t pgn, std::vector data) +{ + std::vector bytes = { 0x80, 0x81, 0x7F, pgn, static_cast(data.size()) }; + bytes.insert(bytes.end(), data.begin(), data.end()); + bytes.push_back(0x00); // CRC, not checked + return bytes; +} + +static aog_packet::ParseResult parse(std::vector &datagram, std::vector &packets) +{ + return aog_packet::parse_datagram(datagram, [&packets](std::uint8_t src, std::uint8_t pgn, std::span data) { + packets.push_back({ src, pgn, std::vector(data.begin(), data.end()) }); + }); +} + +static void test_short_packet_is_parsed_on_its_own() +{ + std::printf("test_short_packet_is_parsed_on_its_own\n"); + + // PGN 241 (section control request) is 7 bytes; it must not wait for the next datagram. + auto datagram = make_packet(0xF1, { 0x01 }); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a 7-byte datagram parses completely"); + check(1 == packets.size(), "one packet is delivered"); + check(!packets.empty() && 0xF1 == packets[0].pgn && 1 == packets[0].data.size() && 0x01 == packets[0].data[0], "PGN and data are correct"); +} + +static void test_short_datagram_does_not_eat_the_next_one() +{ + std::printf("test_short_datagram_does_not_eat_the_next_one\n"); + + // The field failure: a 7-byte PGN 241 was kept in the buffer, the next datagram (PGN 229) was + // appended to it, and the parser then read the second packet from the wrong offset and dropped it. + auto first = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }); + std::vector packets; + parse(first, packets); + auto result = parse(second, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "the following datagram parses completely"); + check(2 == packets.size(), "both packets are delivered"); + check(2 == packets.size() && 0xE5 == packets[1].pgn && 8 == packets[1].data.size() && 0x03 == packets[1].data[0], "the PGN 229 section bits arrive intact"); +} + +static void test_several_packets_in_one_datagram() +{ + std::printf("test_several_packets_in_one_datagram\n"); + + auto datagram = make_packet(0xF1, { 0x01 }); + auto second = make_packet(0xE5, { 0xFF, 0x0F, 0, 0, 0, 0, 0, 0 }); + auto third = make_packet(0xF2, { 0x8D, 0x01, 1, 2, 3, 4 }); + datagram.insert(datagram.end(), second.begin(), second.end()); + datagram.insert(datagram.end(), third.begin(), third.end()); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "parses completely"); + check(3 == packets.size(), "all three packets are delivered"); + check(3 == packets.size() && 0xF1 == packets[0].pgn && 0xE5 == packets[1].pgn && 0xF2 == packets[2].pgn, "in order"); + check(3 == packets.size() && 0x0F == packets[1].data[1] && 4 == packets[2].data[5], "with their own data"); +} + +static void test_unknown_start_stops_without_delivering_garbage() +{ + std::printf("test_unknown_start_stops_without_delivering_garbage\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.insert(datagram.end(), { 0x24, 0x47, 0x50, 0x47, 0x47, 0x41, 0x2C }); // "$GPGGA," after a valid packet + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::UnknownStart == result.stop, "stops at the unknown start"); + check(14 == result.offset, "reports where it stopped"); + check(1 == packets.size(), "the valid packet before it is still delivered"); +} + +static void test_truncated_packet_is_not_delivered() +{ + std::printf("test_truncated_packet_is_not_delivered\n"); + + auto datagram = make_packet(0xE5, { 1, 0, 0, 0, 0, 0, 0, 0 }); + datagram.resize(datagram.size() - 3); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Truncated == result.stop, "reports a truncated packet"); + check(packets.empty(), "delivers nothing past the end of the datagram"); + + std::vector headerOnly = { 0x80, 0x81, 0x7F }; + packets.clear(); + result = parse(headerOnly, packets); + check(aog_packet::ParseStop::Truncated == result.stop, "a partial header is truncated, not read past"); + check(packets.empty(), "and delivers nothing"); +} + +static void test_empty_packet() +{ + std::printf("test_empty_packet\n"); + + auto datagram = make_packet(0xC8, {}); + std::vector packets; + auto result = parse(datagram, packets); + + check(aog_packet::ParseStop::Complete == result.stop, "a zero-length packet parses"); + check(1 == packets.size() && packets[0].data.empty(), "with empty data"); +} + +int main() +{ + test_short_packet_is_parsed_on_its_own(); + test_short_datagram_does_not_eat_the_next_one(); + test_several_packets_in_one_datagram(); + test_unknown_start_stops_without_delivering_garbage(); + test_truncated_packet_is_not_delivered(); + test_empty_packet(); + + if (0 != failures) + { + std::fprintf(stderr, "%d assertion(s) failed\n", failures); + return 1; + } + std::printf("All tests passed\n"); + return 0; +} diff --git a/tools/test_guidance_track.cpp b/tools/test_guidance_track.cpp new file mode 100644 index 0000000..161dceb --- /dev/null +++ b/tools/test_guidance_track.cpp @@ -0,0 +1,173 @@ +/** + * @brief Unit tests for GuidanceTrackProvider, which parses AOG's PGN 0xF4. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "guidance_track_context.hpp" + +#include +#include +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +static std::array make_packet(std::uint8_t sequence, + std::uint8_t flags, + std::uint16_t referenceId, + std::int16_t current, + std::int16_t left, + std::int16_t right) +{ + auto lo = [](std::int16_t v) { return static_cast(static_cast(v) & 0xFF); }; + auto hi = [](std::int16_t v) { return static_cast((static_cast(v) >> 8) & 0xFF); }; + return { sequence, + flags, + static_cast(referenceId & 0xFF), + static_cast(referenceId >> 8), + lo(current), + hi(current), + lo(left), + hi(left), + lo(right), + hi(right) }; +} + +static GuidanceTrackContext parse(GuidanceTrackProvider &provider, const std::array &packet) +{ + return provider.parse(std::span(packet.data(), packet.size())); +} + +static GuidanceTrackContext parse_with_swath_width(GuidanceTrackProvider &provider, std::uint16_t swathWidthMm) +{ + auto base = make_packet(0, 0x01, 1, 0, -1, 1); + std::array packet{}; + std::copy(base.begin(), base.end(), packet.begin()); + packet[10] = static_cast(swathWidthMm & 0xFF); + packet[11] = static_cast(swathWidthMm >> 8); + return provider.parse(std::span(packet.data(), packet.size())); +} + +static void test_valid_packet_fields_and_offset() +{ + std::printf("test_valid_packet_fields_and_offset\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 0x1234, 5, 4, 6)); + + check(ctx.valid, "valid packet is accepted"); + check(ctx.guidanceReferenceLineId == 0x1234, "reference line ID is passed through"); + check(ctx.actualTrackNumber == 6, "actual track number gets the +1 offset"); + check(ctx.trackNumberLeft == 5, "left track number gets the +1 offset"); + check(ctx.trackNumberRight == 7, "right track number gets the +1 offset"); +} + +static void test_swath_width() +{ + std::printf("test_swath_width\n"); + + GuidanceTrackProvider provider; + auto ctx = parse_with_swath_width(provider, 18000); + check(ctx.valid, "12-byte packet is accepted"); + check(ctx.swathWidthMm == 18000, "swath width is decoded as little-endian mm"); + + GuidanceTrackProvider legacyProvider; + auto legacy = parse(legacyProvider, make_packet(0, 0x01, 1, 0, -1, 1)); + check(legacy.valid, "10-byte packet is still accepted"); + check(legacy.swathWidthMm == 0, "10-byte packet reports no swath width"); +} + +static void test_negative_track_numbers() +{ + std::printf("test_negative_track_numbers\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 1, -3, -4, -2)); + + check(ctx.valid, "negative track numbers are valid"); + check(ctx.actualTrackNumber == -2, "-3 + offset = -2"); + check(ctx.trackNumberLeft == -3, "-4 + offset = -3"); + check(ctx.trackNumberRight == -1, "-2 + offset = -1"); +} + +static void test_short_payload_rejected() +{ + std::printf("test_short_payload_rejected\n"); + + GuidanceTrackProvider provider; + const std::array shortPacket = { 0, 0x01, 1, 0, 0, 0, 0, 0, 0 }; + auto ctx = provider.parse(std::span(shortPacket.data(), shortPacket.size())); + + check(!ctx.valid, "a 9-byte payload is rejected"); +} + +static void test_guidance_off() +{ + std::printf("test_guidance_off\n"); + + GuidanceTrackProvider provider; + check(!parse(provider, make_packet(0, 0x00, 7, 1, 0, 2)).valid, "valid flag clear -> invalid"); + check(!parse(provider, make_packet(1, 0x01, 0, 1, 0, 2)).valid, "reference line ID 0 -> invalid"); + check(parse(provider, make_packet(2, 0x01, 7, 1, 0, 2)).valid, "a guidance-off packet still advances the sequence"); +} + +static void test_sequence_freshness() +{ + std::printf("test_sequence_freshness\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "first packet is accepted"); + check(!parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "duplicate sequence is rejected"); + check(!parse(provider, make_packet(9, 0x01, 1, 0, 0, 0)).valid, "older sequence is rejected"); + check(parse(provider, make_packet(11, 0x01, 1, 0, 0, 0)).valid, "next sequence is accepted"); +} + +static void test_sequence_wrap() +{ + std::printf("test_sequence_wrap\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 is accepted"); + check(parse(provider, make_packet(0, 0x01, 1, 0, 0, 0)).valid, "0 after 255 is forward progress"); + check(!parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 after 0 is a step back"); +} + +static void test_reset() +{ + std::printf("test_reset\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "packet accepted"); + provider.reset(); + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "same sequence is accepted again after reset"); +} + +int main() +{ + test_valid_packet_fields_and_offset(); + test_swath_width(); + test_negative_track_numbers(); + test_short_payload_rejected(); + test_guidance_off(); + test_sequence_freshness(); + test_sequence_wrap(); + test_reset(); + + if (failures != 0) + { + std::fprintf(stderr, "%d check(s) failed\n", failures); + return 1; + } + std::printf("All guidance track tests passed\n"); + return 0; +} diff --git a/tools/test_tecu_coexistence.cpp b/tools/test_tecu_coexistence.cpp new file mode 100644 index 0000000..850cb6c --- /dev/null +++ b/tools/test_tecu_coexistence.cpp @@ -0,0 +1,157 @@ +/** + * @brief Unit tests for sharing the bus with other Tractor ECUs (ISO 11783-9:2012, 4.4.2.1): + * which messages a secondary TECU sends, what it advertises, and PGN decoding. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "tecu_coexistence.hpp" + +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +static std::array nothing_sent() +{ + return {}; +} + +static void test_primary_sends_everything() +{ + std::printf("test_primary_sends_everything\n"); + Facilities others; + others.groundBasedSpeed = true; + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::WheelBasedSpeed)] = true; + + const auto plan = plan_tecu_messages(true, others, sent); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + check(plan.send[i], "primary sends every message, whatever others do"); + } +} + +static void test_secondary_alone_sends_everything() +{ + std::printf("test_secondary_alone_sends_everything\n"); + const auto plan = plan_tecu_messages(false, Facilities{}, nothing_sent()); + for (std::size_t i = 0; i < TECU_MESSAGE_COUNT; i++) + { + check(plan.send[i], "secondary with nothing provided by others sends every message"); + } +} + +// The ISO 11783-9 example: the tractor has wheel sensors but no ground-based speed; the display adds it from GNSS +static void test_secondary_adds_ground_speed_only() +{ + std::printf("test_secondary_adds_ground_speed_only\n"); + Facilities tractor; + tractor.wheelBasedSpeed = true; + tractor.wheelBasedDistance = true; + tractor.wheelBasedDirection = true; + tractor.selectedSpeed = true; + tractor.timeDate = true; + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::VehicleDirectionSpeed)] = true; + + const auto plan = plan_tecu_messages(false, tractor, sent); + check(plan.sends(TecuMessage::GroundBasedSpeed), "sends ground-based speed (tractor lacks it)"); + check(!plan.sends(TecuMessage::WheelBasedSpeed), "leaves wheel-based speed to the tractor"); + check(!plan.sends(TecuMessage::MachineSelectedSpeed), "leaves machine selected speed to the tractor"); + check(!plan.sends(TecuMessage::TimeDate), "leaves time/date to the tractor"); + check(!plan.sends(TecuMessage::VehicleDirectionSpeed), "does not double a message the tractor sends without a facility bit"); + check(plan.sends(TecuMessage::CogSogRapidUpdate), "sends COG/SOG, which the tractor neither advertises nor sends"); + + const Facilities ours = facilities_for_plan(plan); + check(ours.tecuClass == 1, "advertises class 1"); + check(ours.groundBasedSpeed && ours.groundBasedDistance && ours.groundBasedDirection, "advertises ground-based speed, distance, direction"); + check(!ours.wheelBasedSpeed && !ours.wheelBasedDistance && !ours.wheelBasedDirection, "does not advertise wheel-based facilities"); + check(!ours.selectedSpeed, "does not advertise selected speed"); + check(!ours.timeDate, "time/date bit is left to the time/date code"); + + // ISO 11783-9 4.4.2.1 d: no facility bit set by both + const auto tractorPayload = encode_facilities(tractor); + const auto ourPayload = encode_facilities(ours); + for (std::size_t i = 1; i < tractorPayload.size(); i++) + { + check(0 == (tractorPayload[i] & ourPayload[i]), "no facility bit advertised by both TECUs"); + } + check(0 == (tractorPayload[0] & ourPayload[0] & 0x3F), "no byte 1 facility bit advertised by both TECUs"); +} + +static void test_seen_on_bus_counts_without_facility_bit() +{ + std::printf("test_seen_on_bus_counts_without_facility_bit\n"); + auto sent = nothing_sent(); + sent[static_cast(TecuMessage::GroundBasedSpeed)] = true; // sent but not advertised + const auto plan = plan_tecu_messages(false, Facilities{}, sent); + check(!plan.sends(TecuMessage::GroundBasedSpeed), "a message seen from another TECU is not sent, advertised or not"); + check(plan.sends(TecuMessage::WheelBasedSpeed), "others are still sent"); +} + +static void test_primary_advertises_selected_speed() +{ + std::printf("test_primary_advertises_selected_speed\n"); + const Facilities ours = facilities_for_plan(plan_tecu_messages(true, Facilities{}, nothing_sent())); + const auto payload = encode_facilities(ours); + check(payload[0] == 0x06, "byte 1: class 1, ground- and wheel-based speed"); + check(payload[2] == 0x78, "byte 3: ground- and wheel-based distance and direction"); + check(payload[4] == 0x04, "byte 5: selected speed (ISO 11783-7 B.24.3 byte 5 bit 3)"); +} + +static void test_merge_facilities() +{ + std::printf("test_merge_facilities\n"); + Facilities a; + a.tecuClass = 1; + a.wheelBasedSpeed = true; + Facilities b; + b.tecuClass = 2; + b.timeDate = true; + b.frontHitchPosition = true; + const Facilities merged = merge_facilities(a, b); + check(merged.wheelBasedSpeed && merged.timeDate && merged.frontHitchPosition, "merge keeps every facility of both"); + check(!merged.groundBasedSpeed, "merge does not invent facilities"); + check(merged.tecuClass == 2, "merge keeps the higher class"); +} + +static void test_pgn_from_can_identifier() +{ + std::printf("test_pgn_from_can_identifier\n"); + check(pgn_from_can_identifier(0x0CFE49F0) == 65097, "ground-based speed from 0xF0"); + check(pgn_from_can_identifier(0x0CFE4826) == 65096, "wheel-based speed from 0x26"); + check(pgn_from_can_identifier(0x0CF022F0) == 61474, "machine selected speed (PDU2, PS is part of the PGN)"); + check(pgn_from_can_identifier(0x18FEE6F0) == 65254, "time/date"); + check(pgn_from_can_identifier(0x09F80281) == 129026, "NMEA 2000 COG/SOG (data page 1)"); + check(pgn_from_can_identifier(0x18EA80F0) == 0xEA00, "PDU1 request: destination address is not part of the PGN"); + check(tecu_message_pgn(TecuMessage::CogSogRapidUpdate) == 129026, "COG/SOG PGN"); +} + +int main() +{ + test_primary_sends_everything(); + test_secondary_alone_sends_everything(); + test_secondary_adds_ground_speed_only(); + test_seen_on_bus_counts_without_facility_bit(); + test_primary_advertises_selected_speed(); + test_merge_facilities(); + test_pgn_from_can_identifier(); + + if (failures > 0) + { + std::fprintf(stderr, "\n%d check(s) FAILED\n", failures); + return 1; + } + std::printf("\nAll checks passed\n"); + return 0; +} diff --git a/tools/test_tractor_facilities.cpp b/tools/test_tractor_facilities.cpp new file mode 100644 index 0000000..f46868f --- /dev/null +++ b/tools/test_tractor_facilities.cpp @@ -0,0 +1,402 @@ +/** + * @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"); +} + +static std::tm make_tm(int year, int month, int day, int yday, int hour, int minute, int second) +{ + std::tm t{}; + t.tm_year = year - 1900; + t.tm_mon = month - 1; + t.tm_mday = day; + t.tm_yday = yday; + t.tm_hour = hour; + t.tm_min = minute; + t.tm_sec = second; + return t; +} + +static void test_utc_offset() +{ + std::printf("[test_utc_offset]\n"); + const auto summerUtc = make_tm(2026, 7, 15, 195, 10, 20, 30); + check(utc_offset_minutes(summerUtc, make_tm(2026, 7, 15, 195, 12, 20, 30)) == 120, "CEST (daylight saving) = +120"); + check(utc_offset_minutes(summerUtc, summerUtc) == 0, "UTC = 0"); + + const auto lateUtc = make_tm(2026, 3, 10, 68, 23, 30, 0); + check(utc_offset_minutes(lateUtc, make_tm(2026, 3, 11, 69, 0, 30, 0)) == 60, "local is already tomorrow"); + const auto earlyUtc = make_tm(2026, 3, 11, 69, 1, 0, 0); + check(utc_offset_minutes(earlyUtc, make_tm(2026, 3, 10, 68, 21, 0, 0)) == -240, "local is still yesterday"); + + const auto newYearEveUtc = make_tm(2026, 12, 31, 364, 23, 30, 0); + check(utc_offset_minutes(newYearEveUtc, make_tm(2027, 1, 1, 0, 0, 30, 0)) == 60, "local is already next year"); + const auto newYearUtc = make_tm(2027, 1, 1, 0, 2, 0, 0); + check(utc_offset_minutes(newYearUtc, make_tm(2026, 12, 31, 364, 22, 30, 0)) == -210, "local is still last year, half-hour zone"); +} + +static void test_time_date_payload() +{ + std::printf("[test_time_date_payload]\n"); + const auto utc = make_tm(2026, 7, 15, 195, 10, 20, 30); + const auto p = encode_time_date(utc, make_tm(2026, 7, 15, 195, 12, 20, 30), 750); + check(p[0] == (30 * 4) + 3, "seconds in 0.25 s steps"); + check(p[1] == 20, "minutes"); + check(p[2] == 10, "hours, UTC"); + check(p[3] == 7, "month"); + check(p[4] == (14 * 4) + 1 + 1, "day in quarter days, second quarter"); + check(p[5] == 41, "years since 1985"); + check(p[6] == 125, "local minute offset 0"); + check(p[7] == 127, "local hour offset +2"); + + const auto newYearUtc = make_tm(2027, 1, 1, 0, 2, 0, 0); + const auto west = encode_time_date(newYearUtc, make_tm(2026, 12, 31, 364, 22, 30, 0), 0); + check(west[6] == 125 - 30, "local minute offset -30"); + check(west[7] == 125 - 3, "local hour offset -3"); + check(west[4] == 1, "first quarter of the first day"); + + const auto leap = encode_time_date(make_tm(2026, 6, 30, 180, 23, 59, 60), make_tm(2026, 6, 30, 180, 23, 59, 60), 999); + check(leap[0] == (59 * 4) + 3, "a leap second stays in range"); +} + +int main() +{ + test_default_ground_speed_only(); + 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(); + test_utc_offset(); + test_time_date_payload(); + + if (failures > 0) + { + std::fprintf(stderr, "\n%d test(s) FAILED\n", failures); + return 1; + } + + std::printf("\nAll tests passed.\n"); + return 0; +}