diff --git a/CMakeLists.txt b/CMakeLists.txt index b49f5af..0b5af7a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,6 +159,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. diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index f7bf925..8610c30 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -95,8 +95,11 @@ All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`, except `0xD6` ( | `0xC9` (201) | Subnet detection | 5 | `[0xC9, 0xC9, IP0, IP1, IP2]` | | `0xD6` (214) | GPS/IMU data | variable (≥39 used) | Only byte 38 (fix quality) is parsed today; rest of frame is currently unused. Source `0x7C`. | | `0xE5` (229) | Section states (64 sections) | 8 | Bitfield: bit `8·j + i` of byte `j` is section `(8j + i)` ON/OFF | +| `0xEF` (239) | Machine data | variable | Only used as an AOG-liveness signal; payload not parsed. | | `0xF1` (241) | Section control mode | 1 | `[mode]` where `1` = enabled, `0` = disabled | | `0xF2` (242) | Process data | 6 | `[DDI_lo, DDI_hi, val0, val1, val2, val3]` — DDI is little-endian `uint16`; value is little-endian `int32` | +| `0xF3` (243) | Field name | variable | The open field's name as raw UTF-8; empty = field closed. | +| `0xF4` (244) | Guidance track context | 10 or 12 | Real-time AB-line/track guidance state, announced to implements as DDI 507-513 — see §5.4.1. | #### `0xC9` — Subnet detection @@ -126,10 +129,34 @@ Wraps a single ISO 11783 DDI/value pair. The TC currently dispatches on these DD |---|---|---| | `156` | Actual speed (mm/s) | Stored. If TECU enabled, broadcast as Ground/Wheel/Machine-selected speed (PGN 65256) + NMEA2000 SOG. Drives forward/reverse direction. Also produces J1939 PGN 65256 every 100 ms. | | `597` | Total distance (mm) | Stored and displayed on the VT Status page. If TECU is enabled, also populated into Speed Messages distance fields. | -| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. | +| Guidance line deviation | XTE (mm) | Converted to metres. Broadcast as NMEA2000 XTE (PGN 0x1F903) at 1 Hz. Also announced to implements as DDI 513 (GuidanceLineDeviation) — see §5.4.1. | Unknown DDIs are silently ignored (PGN 0xF2 is the generic process-data channel — the TC will gain more DDIs over time). +#### `0xF3` — Field name + +The whole payload is the open field's name as raw UTF-8 — no length prefix, no offset — up to 248 bytes (longer names are truncated). An empty payload means the field was closed, which also invalidates the current track context. + +The TC maps each field name to a persistent 16-bit index, stored one `index,name` line per field in `field_registry.csv` next to `settings.json`. The index is folded into the upper 16 bits of DDI 508 (see `0xF4`), so a track's reference line ID stays unique across fields. + +#### `0xF4` — Guidance track context + +AOG's real-time AB-line/track guidance state. 12-byte payload (a 10-byte payload without the last field is also accepted): + +``` + Byte 0 Sequence counter (0–255, wraps) + Byte 1 Flags: bit0=valid, bit1=heading same way, bit2=curve mode + Bytes 2-3 Guidance Reference Line ID (uint16 LE) — 0 = no active track + Bytes 4-5 Actual Track Number (int16 LE, signed — can be negative and jump by more than 1) + Bytes 6-7 Track Number Left (int16 LE, signed) + Bytes 8-9 Track Number Right (int16 LE, signed) + Bytes 10-11 Swath Width in mm (uint16 LE) — distance between adjacent tracks; 0 = not reported +``` + +AOG sends this **only when the guidance state actually changes** — there is no heartbeat. The TC rejects any packet whose sequence number isn't strictly ahead of the last accepted one (catches duplicates, freezes, and reordered/stale UDP delivery). + +**Track-number offset:** the TC adds `+1` to all three track numbers (current/left/right) before announcing them — confirmed by field testing, not documented anywhere on AOG's side. As sent raw by AOG, a tramline implement's own on-board phase (which pass of N is "on") was consistently one pass out of sync with AOG's own intended on/off state, for both left and right passes; a uniform `+1` (independent of sign) brought them into agreement. See `GuidanceTrackProvider::parse()` (`AOG_TRACK_NUMBER_OFFSET`). + ### 2.6 PGNs outbound (TC → client) All PGNs sent **by the TC to AgIO/AgValonia** use source `0x80`. @@ -261,6 +288,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). | | NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. | | GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | AOG's GPS fix quality (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` when no fresh fix quality is available. | +| Guidance track data (DDI 507-513, via `0xCB00` Process Data) | 250 ms, while AOG has a valid track | TC | Track number, adjacent tracks, reference line, swath width and line deviation from AOG's PGN `0xF4`/`0xF2`, sent to each client whose DDOP declares them as settable. See §5.4.1. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. @@ -281,6 +309,22 @@ The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control co | Max sections | 64 | | Supported DDIs | 160 / 161 / 290 (condensed section setpoint and actual states), plus speed/distance/guidance DDIs from the tractor side | +#### 5.4.1 Guidance data sent to implements + +The TC pushes guidance data to each client whose DDOP declares the DDI as settable. Clients can't request values from the TC, so a DDI a client doesn't declare, or declares as not settable, is never sent to it. Only clients with sections (implements, not tractors) are considered, as with the other DDI mappings. + +| DDI | Name | Sent when | +|---|---|---| +| 507 | GuidanceTrackSequenceNumber | Valid track. Increments whenever the actual track number or the reference line ID changes — not on section-control toggles. | +| 508 | UniqueGuidanceReferenceLineID | Valid track. AOG's 16-bit reference line ID in the low 16 bits, the persistent field index (see `0xF3`) in the high 16 bits. | +| 509 | ActualGuidanceTrackNumber | Valid track. Signed; can be negative and can jump by more than 1 in a single update (e.g. skipping several tracks on a headland turn). | +| 510 / 511 | GuidanceTrackNumberToTheRight / ...ToTheLeft | Valid track. | +| 512 | GuidanceLineSwathWidth | Valid track, and AOG reported a non-zero swath width in `0xF4`. AOG's track spacing (tool width minus overlap), the same spacing the track numbers are derived from. | +| 513 | GuidanceLineDeviation | Valid track. AOG's XTE in mm (PGN `0xF2`, see §2.5). | +| 514 | GNSSQuality | Always — see `0xD6`. | + +"Valid track" means the TC holds an accepted PGN `0xF4` payload with the valid flag set, a non-zero reference line ID, and an open field (PGN `0xF3`) to scope the ID to. Since AOG only sends `0xF4` on change, validity is *not* cleared just because no new `0xF4` has arrived; it is cleared only by an explicit "guidance off" packet, by the field closing, or by AOG disconnecting entirely (no packets of any kind for 3 s). + ### 5.5 Virtual Terminal UI The roughly 12 KB VT object pool is embedded in the executable, so deployment does not require a separate `AOG_TC.iop` file. The committed `src/AOG_TC.iop` remains its build-time source of truth, while `src/AOG_TC.iop.h` contains the ISO-Designer-generated object IDs and authored geometry. diff --git a/include/app.hpp b/include/app.hpp index 6ee3ca7..ffe0a04 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -22,6 +22,8 @@ #include "isobus/isobus/isobus_virtual_terminal_client_update_helper.hpp" #include "isobus/isobus/nmea2000_message_interface.hpp" +#include "field_registry.hpp" +#include "guidance_track_context.hpp" #include "logging_utils.hpp" #include "settings.hpp" #include "task_controller.hpp" @@ -76,6 +78,8 @@ class Application static constexpr std::uint8_t HW_MSG_ALERT = 0; static constexpr std::uint8_t HW_MSG_INFO = 1; + bool is_aog_connected() const; + std::shared_ptr settings = std::make_shared(); boost::asio::io_context ioContext = boost::asio::io_context(); std::shared_ptr udpConnections = std::make_shared(settings, ioContext); @@ -105,6 +109,21 @@ class Application std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated std::uint32_t lastGnssQualityMs = 0; ///< Timestamp of last PGN 0xD6 fix-quality update (0 = never received) static constexpr std::uint32_t GNSS_QUALITY_TIMEOUT_MS = 2000; ///< No PGN 0xD6 for this long = fix quality unknown + static constexpr std::uint32_t AOG_CONNECTION_TIMEOUT_MS = 3000; ///< No AOG packet for this long = disconnected + + // Guidance track context — real data from AOG PGN 0xF4. + GuidanceTrackProvider trackProvider; + GuidanceTrackContext currentTrackContext; + bool aogWasConnectedForTrack = false; ///< Edge-detection for AOG connect/disconnect transitions + + // Field identity — from AOG PGN 0xF3. Folded into the upper 16 bits of DDI 508 + // (see the PGN 0xF4 handling in setup_udp_connections()) so a track's guidance reference + // line ID is unique across fields, not just within whichever field AOG currently has open. + FieldRegistry fieldRegistry; + std::string currentFieldName; ///< Empty when no field is open + std::uint16_t currentFieldIndex = 0; + bool hasActiveField = false; + std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; std::uint32_t vtDisconnectedSinceMs = 0; diff --git a/include/field_registry.hpp b/include/field_registry.hpp new file mode 100644 index 0000000..13f3720 --- /dev/null +++ b/include/field_registry.hpp @@ -0,0 +1,50 @@ +/** + * @file field_registry.hpp + * @brief Persistent field-name -> field-index mapping, used to make ISOBUS TRACK's + * DDI 508 (Unique Guidance Reference Line ID) actually unique across fields. + * + * AOG's own PGN 0xF4 guidance reference ID is only a 16-bit value scoped to whatever + * field is currently open in AOG - it is not guaranteed unique across different fields. + * An implement that caches per-track state (e.g. an offset) keyed on DDI 508 alone can + * therefore collide across a field switch. This registry assigns each field name a + * stable index, which the caller folds into the upper 16 bits of the 32-bit DDI 508 + * value (see Application's PGN 0xF3/0xF4 handling), leaving AOG's own 16-bit ID in the + * lower 16 bits untouched. + */ + +#pragma once + +#include +#include +#include + +/// @brief Loads/persists a field-name -> field-index mapping from a plain text file. +/// +/// Deliberately its own file rather than part of settings.json: the mapping only grows +/// over time (one line per field ever opened), so a user may want to wipe it on its own +/// (e.g. to reclaim indices) without touching the rest of their configuration. +class FieldRegistry +{ +public: + /// @brief Loads the registry from disk, if present. A missing or unreadable file + /// just starts empty - fields get freshly (re-)indexed and persisted as they're seen. + FieldRegistry(); + + /// @brief Returns the persistent index for a field name, assigning and persisting a + /// new one the first time this name is seen. + /// @param fieldName UTF-8 field folder name, as received from AOG PGN 0xF3. + /// @returns A stable index. Once the 16-bit space is exhausted (65536 distinct + /// field names - far beyond realistic use), the most recently assigned index is + /// reused and a warning is logged, rather than silently colliding with an existing + /// field. + std::uint16_t get_or_assign_index(const std::string &fieldName); + +private: + void load(); + void append_entry(const std::string &fieldName, std::uint16_t index); + + std::string filePath; + std::unordered_map nameToIndex; + std::uint16_t nextIndex = 0; + bool nextIndexExhausted = false; +}; diff --git a/include/guidance_track_context.hpp b/include/guidance_track_context.hpp new file mode 100644 index 0000000..6240e16 --- /dev/null +++ b/include/guidance_track_context.hpp @@ -0,0 +1,85 @@ +/** + * @file guidance_track_context.hpp + * @brief Abstraction layer between AOG input and ISOBUS TRACK (Generation 1) protocol. + * + * This header defines the GuidanceTrackContext struct and the GuidanceTrackProvider + * that consumes AOG PGN 0xF4 guidance-track data. + * + * The ISOBUS TRACK sender consumes a GuidanceTrackContext without caring how it + * was produced. The GuidanceTrackContext maps directly to ISOBUS DDIs 508–511. + */ + +#pragma once + +#include +#include +#include + +/** + * @brief Immutable snapshot of guidance-track state consumed by the ISOBUS TRACK sender. + * + * Corresponds to ISOBUS DDIs: + * - guidanceReferenceLineId -> DDI 508 (Unique Guidance Reference Line ID) + * - actualTrackNumber -> DDI 509 (Actual Guidance Track Number) + * - trackNumberRight -> DDI 510 (Guidance Track Number to the Right) + * - trackNumberLeft -> DDI 511 (Guidance Track Number to the Left) + * - swathWidthMm -> DDI 512 (Guidance Line Swath Width) + */ +struct GuidanceTrackContext +{ + std::uint32_t guidanceReferenceLineId = 1; ///< DDI 508 + std::int32_t actualTrackNumber = 0; ///< DDI 509 — signed; track 0 is valid + std::int32_t trackNumberRight = -1; ///< DDI 510 + std::int32_t trackNumberLeft = 1; ///< DDI 511 + std::uint32_t swathWidthMm = 0; ///< DDI 512 — distance between adjacent tracks; 0 = not reported by AOG + bool valid = false; ///< Context has been initialized with at least one update +}; + +/** + * @brief Real guidance-track provider that consumes AOG PGN 0xF4 (244) data. + * + * PGN 0xF4 payload layout (10 data bytes, optionally 12): + * Byte 0: Sequence counter (0–255, wrapping) + * Byte 1: Flags (bit 0 = valid, bit 1 = heading same way, bit 2 = curve mode) + * Bytes 2-3: Guidance Reference ID (uint16 LE) + * Bytes 4-5: Current Track Number (int16 LE, signed) + * Bytes 6-7: Track Number Left (int16 LE, signed) + * Bytes 8-9: Track Number Right (int16 LE, signed) + * Bytes 10-11: Swath Width in mm (uint16 LE), optional — only present from newer AOG builds + * + * Track numbers (current/left/right) are each shifted by +1 from the raw wire value + * (see AOG_TRACK_NUMBER_OFFSET in parse()) to match the tram-pattern phase implements + * expect — confirmed by field testing, not part of AOG's own documented wire format. + * + * Sequence tracking: + * - Rejects any packet whose sequence number is not strictly ahead of the last + * accepted one (signed delta over the 0-255 wrap), catching both frozen/duplicate + * data and reordered/stale UDP packets arriving out of order. + * - Call reset() after a disconnect to treat the next packet as a fresh start. + * + * Returns valid=true when data is valid, refId != 0, and sequence is fresh. + */ +class GuidanceTrackProvider +{ +public: + /// Minimum payload size (10 data bytes) + static constexpr std::size_t MIN_PAYLOAD_SIZE = 10; + + /// Payload size that includes the swath width + static constexpr std::size_t PAYLOAD_SIZE_WITH_SWATH_WIDTH = 12; + + /** + * @brief Parse AOG PGN 0xF4 payload and produce a GuidanceTrackContext. + * + * @param data Payload bytes (after UDP header stripping) + * @return GuidanceTrackContext with valid=true if parse succeeded and flags indicate valid data + */ + GuidanceTrackContext parse(std::span data); + + /// @brief Reset sequence tracking (e.g., after AOG disconnect timeout). + /// The next parse() call is treated as a fresh start — no delta comparison. + void reset(); + +private: + std::optional lastSequence_; ///< Unset until the first packet is accepted +}; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 839a4fc..517f17a 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -9,6 +9,7 @@ #pragma once +#include "guidance_track_context.hpp" #include "isobus/isobus/isobus_data_dictionary.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool.hpp" #include "isobus/isobus/isobus_standard_data_description_indices.hpp" @@ -63,6 +64,11 @@ class ClientState void set_element_work_state(std::uint16_t elementNumber, bool isWorking); bool try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const; + /// @brief Advances the DDI 507 sequence number if the track or reference line differs from the + /// last one announced to this client, and remembers the new values. + /// @returns The sequence number to announce. + std::uint32_t update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId); + private: isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC bool areMeasurementCommandsSent = false; ///< Whether or not the measurement commands have been sent @@ -79,6 +85,9 @@ class ClientState bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting bool usesPerElementControl = false; ///< Legacy mode: use per-element setpoint instead of condensed std::uint16_t perElementSetpointDDI = 0; ///< The DDI to use for per-element setpoints (289 or 141), 0 if not applicable + std::int32_t lastSentTrackNumber = 0; ///< Last track number announced, for DDI 507 change detection + std::uint32_t lastSentReferenceLineId = 0; ///< Last reference line ID announced, for DDI 507 change detection + std::uint32_t guidanceTrackSequenceNumber = 0; ///< Per-client DDI 507 sequence number }; // Create the task controller server object, this will handle all the ISOBUS communication for us @@ -120,6 +129,12 @@ class MyTCServer : public isobus::TaskControllerServer /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated void send_gnss_quality(std::uint8_t quality); + /// @brief Announces the current guidance track (DDI 507-511) and line deviation (DDI 513) to every + /// client that declares those DDIs. Does nothing while the context is not valid. + /// @param ctx Current guidance track state from AOG + /// @param lineDeviationMm Deviation from the guidance line in mm + void send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm); + private: void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); diff --git a/readme.md b/readme.md index f525e63..734e5e2 100644 --- a/readme.md +++ b/readme.md @@ -96,6 +96,7 @@ The application logs the detected VT version, screen size, softkey dimensions, a Sent to any implement whose device description declares the DDI as settable. - **GNSS quality (DDI 514):** AgOpenGPS's GPS fix quality, every 250 ms. +- **Guidance track (DDI 507-513):** the current track number, the tracks to its left and right, the track spacing, a reference line ID that is unique across fields, and the deviation from the guidance line, every 250 ms while AgOpenGPS has an active track. ## Contributing diff --git a/src/app.cpp b/src/app.cpp index 38b15ae..51956c0 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -510,6 +510,82 @@ void Application::setup_udp_connections() log() << "Received request from AOG to change section control state to " << (sectionControlState == 1 ? "enabled" : "disabled") << std::endl; tcServer->update_section_control_enabled(sectionControlState == 1); } + else if (pgn == 0xEF) // 239 - Machine Data + { + // Not parsed; only counts as AOG liveness. + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + } + else if (pgn == 0xF3) // 243 - Field Name + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // The whole payload IS the UTF-8 field name — no length prefix, no offset. + // Confirmed against a live packet: a documented "length byte at offset 4, + // name at offset 5+" layout does not match what AOG actually sends — the + // payload was exactly N raw UTF-8 name bytes, nothing else. An empty + // payload means the field is closed. + if (data.empty()) + { + if (hasActiveField) + { + log("Field") << "Field closed: " << currentFieldName << std::endl; + } + currentFieldName.clear(); + hasActiveField = false; + // Invalidate any in-flight track context immediately — broadcasting + // DDI 508 without a field to scope it to would defeat the point of the + // field index folded into it below. + currentTrackContext.valid = false; + } + else + { + constexpr std::size_t MAX_FIELD_NAME_BYTES = 248; + std::size_t nameLength = data.size(); + if (nameLength > MAX_FIELD_NAME_BYTES) + { + log("Field") << "PGN 0xF3 name of " << nameLength << " bytes exceeds the documented " + << MAX_FIELD_NAME_BYTES << "-byte max; truncating." << std::endl; + nameLength = MAX_FIELD_NAME_BYTES; + } + + std::string fieldName(reinterpret_cast(data.data()), nameLength); + if (fieldName != currentFieldName || !hasActiveField) + { + currentFieldName = fieldName; + currentFieldIndex = fieldRegistry.get_or_assign_index(fieldName); + hasActiveField = true; + log("Field") << "Field opened: " << currentFieldName << " (index " << currentFieldIndex << ")" << std::endl; + } + } + } + else if (pgn == 0xF4) // 244 - Guidance Track Context + { + lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); + + // Always update currentTrackContext: when AOG sends valid=false + // (guidance off / no active track), the context must be invalidated + // so the TC stops broadcasting stale track data. + // Note: parse() handles short-payload validation internally. + currentTrackContext = trackProvider.parse(data); + + // AOG's own guidance reference ID (see GuidanceTrackProvider) is only unique + // within whichever field AOG currently has open — fold in the field's own + // persistent index (upper 16 bits) so DDI 508 is unique across fields too. + // Without an active field, there's nothing to scope the ID to — don't send it. + if (currentTrackContext.valid) + { + if (hasActiveField) + { + currentTrackContext.guidanceReferenceLineId = + (static_cast(currentFieldIndex) << 16) | + (currentTrackContext.guidanceReferenceLineId & 0xFFFFu); + } + else + { + currentTrackContext.valid = false; + } + } + } else if (pgn == 0xF2 && data.size() >= 6) // Process Data { lastAogPacketMs = isobus::SystemTiming::get_timestamp_ms(); @@ -815,9 +891,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()) @@ -1257,7 +1357,7 @@ void Application::update_vt_client() sync_vt_config_once(); - const bool aogConnected = (lastAogPacketMs != 0) && !isobus::SystemTiming::time_expired_ms(lastAogPacketMs, 3000); + const bool aogConnected = is_aog_connected(); vtUpdateHelper->set_numeric_value(VTSpeedValue, aogConnected ? static_cast(std::abs(lastSpeedValue)) : 0U); vtUpdateHelper->set_numeric_value(VTXteValue, aogConnected ? (static_cast(lastXteValue) ^ 0x80000000U) : 0x80000000U); diff --git a/src/field_registry.cpp b/src/field_registry.cpp new file mode 100644 index 0000000..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/task_controller.cpp b/src/task_controller.cpp index 3af8b84..70f57e6 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -66,6 +66,25 @@ static bool find_owning_element_number(isobus::DeviceDescriptorObjectPool &pool, return false; } +// The DDIs the TC pushes to implements as guidance data. +static bool is_guidance_data_ddi(std::uint16_t ddi) +{ + switch (static_cast(ddi)) + { + case isobus::DataDescriptionIndex::GNSSQuality: + case isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber: + case isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID: + case isobus::DataDescriptionIndex::ActualGuidanceTrackNumber: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight: + case isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft: + case isobus::DataDescriptionIndex::GuidanceLineSwathWidth: + case isobus::DataDescriptionIndex::GuidanceLineDeviation: + return true; + default: + return false; + } +} + void ClientState::set_number_of_sections(std::uint8_t number) { numberOfSections = number; @@ -356,6 +375,18 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool & return false; } +std::uint32_t ClientState::update_guidance_track_sequence(std::int32_t trackNumber, std::uint32_t referenceLineId) +{ + // A line switch can land on the same track index, so compare the reference line too. + if ((trackNumber != lastSentTrackNumber) || (referenceLineId != lastSentReferenceLineId)) + { + guidanceTrackSequenceNumber++; + lastSentTrackNumber = trackNumber; + lastSentReferenceLineId = referenceLineId; + } + return guidanceTrackSequenceNumber; +} + MyTCServer::MyTCServer(std::shared_ptr internalControlFunction, isobus::TaskControllerServer::TaskControllerVersion version) : TaskControllerServer(internalControlFunction, @@ -861,8 +892,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); @@ -872,22 +903,22 @@ void MyTCServer::request_measurement_commands() } auto processDataObject = std::dynamic_pointer_cast(object); - if (!processDataObject || processDataObject->get_ddi() != static_cast(isobus::DataDescriptionIndex::GNSSQuality)) + if (!processDataObject || !is_guidance_data_ddi(processDataObject->get_ddi())) { continue; } if (!processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) { - 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; } } @@ -977,6 +1008,42 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality) } } +void MyTCServer::send_guidance_track_data(const GuidanceTrackContext &ctx, std::int32_t lineDeviationMm) +{ + if (!ctx.valid) + { + return; + } + + std::lock_guard lock(clientsMutex); + for (auto &client : clients) + { + auto &state = client.second; + + auto trySend = [&](isobus::DataDescriptionIndex ddi, std::int32_t value) { + if (state.has_element_number_for_ddi(ddi)) + { + send_set_value(client.first, static_cast(ddi), state.get_element_number_for_ddi(ddi), value); + } + }; + + const std::uint32_t sequenceNumber = state.update_guidance_track_sequence(ctx.actualTrackNumber, ctx.guidanceReferenceLineId); + + // Coherent ordering per the TRACK guideline: + // 507 (sequence) -> 508 (ref line ID) -> 509 (actual track) -> 510 (right) -> 511 (left) + trySend(isobus::DataDescriptionIndex::GuidanceTrackSequenceNumber, static_cast(sequenceNumber)); + trySend(isobus::DataDescriptionIndex::UniqueGuidanceReferenceLineID, static_cast(ctx.guidanceReferenceLineId)); + trySend(isobus::DataDescriptionIndex::ActualGuidanceTrackNumber, ctx.actualTrackNumber); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheRight, ctx.trackNumberRight); + trySend(isobus::DataDescriptionIndex::GuidanceTrackNumberToTheLeft, ctx.trackNumberLeft); + if (ctx.swathWidthMm != 0) + { + trySend(isobus::DataDescriptionIndex::GuidanceLineSwathWidth, static_cast(ctx.swathWidthMm)); + } + trySend(isobus::DataDescriptionIndex::GuidanceLineDeviation, lineDeviationMm); + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex); diff --git a/tools/test_guidance_track.cpp b/tools/test_guidance_track.cpp new file mode 100644 index 0000000..161dceb --- /dev/null +++ b/tools/test_guidance_track.cpp @@ -0,0 +1,173 @@ +/** + * @brief Unit tests for GuidanceTrackProvider, which parses AOG's PGN 0xF4. + * + * Returns 0 when every assertion passes, 1 otherwise. + */ + +#include "guidance_track_context.hpp" + +#include +#include +#include +#include + +static int failures = 0; + +static void check(bool condition, const char *label) +{ + if (!condition) + { + std::fprintf(stderr, " FAIL: %s\n", label); + ++failures; + } +} + +static std::array make_packet(std::uint8_t sequence, + std::uint8_t flags, + std::uint16_t referenceId, + std::int16_t current, + std::int16_t left, + std::int16_t right) +{ + auto lo = [](std::int16_t v) { return static_cast(static_cast(v) & 0xFF); }; + auto hi = [](std::int16_t v) { return static_cast((static_cast(v) >> 8) & 0xFF); }; + return { sequence, + flags, + static_cast(referenceId & 0xFF), + static_cast(referenceId >> 8), + lo(current), + hi(current), + lo(left), + hi(left), + lo(right), + hi(right) }; +} + +static GuidanceTrackContext parse(GuidanceTrackProvider &provider, const std::array &packet) +{ + return provider.parse(std::span(packet.data(), packet.size())); +} + +static GuidanceTrackContext parse_with_swath_width(GuidanceTrackProvider &provider, std::uint16_t swathWidthMm) +{ + auto base = make_packet(0, 0x01, 1, 0, -1, 1); + std::array packet{}; + std::copy(base.begin(), base.end(), packet.begin()); + packet[10] = static_cast(swathWidthMm & 0xFF); + packet[11] = static_cast(swathWidthMm >> 8); + return provider.parse(std::span(packet.data(), packet.size())); +} + +static void test_valid_packet_fields_and_offset() +{ + std::printf("test_valid_packet_fields_and_offset\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 0x1234, 5, 4, 6)); + + check(ctx.valid, "valid packet is accepted"); + check(ctx.guidanceReferenceLineId == 0x1234, "reference line ID is passed through"); + check(ctx.actualTrackNumber == 6, "actual track number gets the +1 offset"); + check(ctx.trackNumberLeft == 5, "left track number gets the +1 offset"); + check(ctx.trackNumberRight == 7, "right track number gets the +1 offset"); +} + +static void test_swath_width() +{ + std::printf("test_swath_width\n"); + + GuidanceTrackProvider provider; + auto ctx = parse_with_swath_width(provider, 18000); + check(ctx.valid, "12-byte packet is accepted"); + check(ctx.swathWidthMm == 18000, "swath width is decoded as little-endian mm"); + + GuidanceTrackProvider legacyProvider; + auto legacy = parse(legacyProvider, make_packet(0, 0x01, 1, 0, -1, 1)); + check(legacy.valid, "10-byte packet is still accepted"); + check(legacy.swathWidthMm == 0, "10-byte packet reports no swath width"); +} + +static void test_negative_track_numbers() +{ + std::printf("test_negative_track_numbers\n"); + + GuidanceTrackProvider provider; + auto ctx = parse(provider, make_packet(0, 0x01, 1, -3, -4, -2)); + + check(ctx.valid, "negative track numbers are valid"); + check(ctx.actualTrackNumber == -2, "-3 + offset = -2"); + check(ctx.trackNumberLeft == -3, "-4 + offset = -3"); + check(ctx.trackNumberRight == -1, "-2 + offset = -1"); +} + +static void test_short_payload_rejected() +{ + std::printf("test_short_payload_rejected\n"); + + GuidanceTrackProvider provider; + const std::array shortPacket = { 0, 0x01, 1, 0, 0, 0, 0, 0, 0 }; + auto ctx = provider.parse(std::span(shortPacket.data(), shortPacket.size())); + + check(!ctx.valid, "a 9-byte payload is rejected"); +} + +static void test_guidance_off() +{ + std::printf("test_guidance_off\n"); + + GuidanceTrackProvider provider; + check(!parse(provider, make_packet(0, 0x00, 7, 1, 0, 2)).valid, "valid flag clear -> invalid"); + check(!parse(provider, make_packet(1, 0x01, 0, 1, 0, 2)).valid, "reference line ID 0 -> invalid"); + check(parse(provider, make_packet(2, 0x01, 7, 1, 0, 2)).valid, "a guidance-off packet still advances the sequence"); +} + +static void test_sequence_freshness() +{ + std::printf("test_sequence_freshness\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "first packet is accepted"); + check(!parse(provider, make_packet(10, 0x01, 1, 0, 0, 0)).valid, "duplicate sequence is rejected"); + check(!parse(provider, make_packet(9, 0x01, 1, 0, 0, 0)).valid, "older sequence is rejected"); + check(parse(provider, make_packet(11, 0x01, 1, 0, 0, 0)).valid, "next sequence is accepted"); +} + +static void test_sequence_wrap() +{ + std::printf("test_sequence_wrap\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 is accepted"); + check(parse(provider, make_packet(0, 0x01, 1, 0, 0, 0)).valid, "0 after 255 is forward progress"); + check(!parse(provider, make_packet(255, 0x01, 1, 0, 0, 0)).valid, "255 after 0 is a step back"); +} + +static void test_reset() +{ + std::printf("test_reset\n"); + + GuidanceTrackProvider provider; + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "packet accepted"); + provider.reset(); + check(parse(provider, make_packet(50, 0x01, 1, 0, 0, 0)).valid, "same sequence is accepted again after reset"); +} + +int main() +{ + test_valid_packet_fields_and_offset(); + test_swath_width(); + test_negative_track_numbers(); + test_short_payload_rejected(); + test_guidance_off(); + test_sequence_freshness(); + test_sequence_wrap(); + test_reset(); + + if (failures != 0) + { + std::fprintf(stderr, "%d check(s) failed\n", failures); + return 1; + } + std::printf("All guidance track tests passed\n"); + return 0; +}