From 7d05e40bdb0f88875e7e5c1cad8521bab2ff3bd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 19 Sep 2026 23:41:44 +0200 Subject: [PATCH] Send GNSS quality (DDI 514) to implements Parses the fix-quality byte from AOG's GPS/IMU PGN 0xD6 (source 0x7C) and pushes it to every client whose DDOP declares DDI 514 (GNSSQuality), every 250 ms. - AOG fix values 0-8 are forwarded unchanged (they are the NMEA 2000 GNSS Method values DDI 514 uses, including 6=Estimated, 7=Manual, 8=Simulated). Values above 8 and a stale/missing 0xD6 map to 1 (weakest real fix) rather than 0, since some implements gate TRACK and section control on GNSS quality being non-zero. - DDI 514 is mapped to its element in request_measurement_commands() alongside the other DDI mappings; mapping only, no OnChange subscription, since it is a value the TC pushes rather than one it reads back. Only implements that declare DDI 514 as settable are mapped, since a non-settable DDI cannot accept a Value command. - docs/PROTOCOL.md and the readme document the new PGN and DDI. The readme gets its own "Guidance data sent to implements" subsection so follow-up guidance PRs append bullets there instead of all editing the shared capabilities list. Co-Authored-By: Claude Sonnet 5 --- docs/PROTOCOL.md | 13 ++++++- include/app.hpp | 3 ++ include/task_controller.hpp | 4 ++ readme.md | 6 +++ src/app.cpp | 48 ++++++++++++++++++++++++ src/task_controller.cpp | 75 +++++++++++++++++++++++++++++++++++++ 6 files changed, 147 insertions(+), 2 deletions(-) diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md index e9d22e8..f7bf925 100644 --- a/docs/PROTOCOL.md +++ b/docs/PROTOCOL.md @@ -48,7 +48,7 @@ Every packet — both directions — uses the same frame: 5+N 1 Checksum Sum of bytes [Source .. last payload byte], mod 256 ``` -Total wire size is `N + 6` bytes. The maximum payload is currently 250 bytes (limited by the TC's 512-byte receive buffer; in practice the largest PGN in use is 8 bytes). +Total wire size is `N + 6` bytes. The maximum payload is currently 250 bytes (limited by the TC's 512-byte receive buffer). Most PGNs are ≤10 bytes; the outlier is `0xD6` (GPS/IMU data, see §2.5), which needs at least 39 bytes today. **Checksum**: the TC currently does **not** validate inbound checksums (the verification code is present but commented out in `udp_connections.cpp`). Clients **should still compute and include a correct checksum** so that future TC versions, or third-party listeners, can validate. @@ -58,6 +58,7 @@ Source byte identifies the logical sender of a frame. The conventions used today | Source | Logical sender | |---|---| +| `0x7C` (124) | **AOG's GPS/IMU submodule** — sends PGN `0xD6` only (see §2.5). | | `0x7F` (127) | **AgIO / AgValonia** (the GUI/host application) | | `0x80` (128) | **AOG-TaskController** itself | @@ -87,11 +88,12 @@ If no NIC matches, the TC falls back to loopback (`127.0.0.1`) — useful for lo ### 2.5 PGNs inbound (client → TC) -All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`. +All PGNs sent **by AgIO/AgValonia to the TC** use source `0x7F`, except `0xD6` (GPS/IMU data), which comes from AOG's GPS submodule at source `0x7C`. | PGN | Name | Length | Payload | |---|---|---|---| | `0xC9` (201) | Subnet detection | 5 | `[0xC9, 0xC9, IP0, IP1, IP2]` | +| `0xD6` (214) | GPS/IMU data | variable (≥39 used) | Only byte 38 (fix quality) is parsed today; rest of frame is currently unused. Source `0x7C`. | | `0xE5` (229) | Section states (64 sections) | 8 | Bitfield: bit `8·j + i` of byte `j` is section `(8j + i)` ON/OFF | | `0xF1` (241) | Section control mode | 1 | `[mode]` where `1` = enabled, `0` = disabled | | `0xF2` (242) | Process data | 6 | `[DDI_lo, DDI_hi, val0, val1, val2, val3]` — DDI is little-endian `uint16`; value is little-endian `int32` | @@ -102,6 +104,12 @@ Tells the TC which `/24` subnet AgIO/AgValonia lives on. The first two payload b On receipt the TC sets `settings.subnet = [IP0, IP1, IP2]`, closes the main socket, re-runs NIC enumeration, and rebinds. Useful for plug-and-play scenarios where the host may move between subnets. +#### `0xD6` — GPS/IMU data + +Sent from AOG's GPS submodule, source `0x7C` (not `0x7F`). The TC only reads byte 38: AOG's fix-quality code (`0`=invalid, `1`=GPS, `2`=DGPS, `3`=PPS, `4`=RTK Fixed, `5`=RTK Float, `6`=Estimated, `7`=Manual, `8`=Simulated — the NMEA 2000 GNSS Method values DDI 514 uses). Values `0`–`8` are forwarded unchanged to implements as DDI 514 (GNSSQuality) — see §5.2. + +Two fallback cases both resolve to `1` (weakest real GNSS fix), not `0` (No GNSS): AOG reporting a value above `8` (not a defined GNSS Method), and no fresh `0xD6` (AOG doesn't always send this PGN at all — e.g. Simulator mode — and if none has arrived within 2 s the last value is treated as stale). `0` is deliberately avoided as a fallback because some implements gate TRACK/section control on GNSS quality being non-zero. + #### `0xE5` — Section states Reports the *actual* state of up to 64 sections. 8 bytes = 64 bits, one bit per section. The TC forwards these to the connected ISOBUS implement via the appropriate condensed work-state DDIs (DDI 160/161/290). @@ -252,6 +260,7 @@ Common NAME fields: Industry Group `2` (Agricultural), Device Class `0`, Manufac | `0xFEE8` (PGN 65256 Speed/Direction) | 100 ms | TECU | Ground/Wheel/Machine-selected speed + machine direction, J1939 format. Only when TECU enabled. | | `0xFC8E` (Control Function Functionalities) | At claim + periodic | TECU | Announces Class 1 BasicTractorECUServer (no options). | | NMEA2000 COG/SOG | Periodic | TECU | Optional course/speed over ground. | +| GNSS Quality (DDI 514, via `0xCB00` Process Data) | 250 ms | TC | AOG's GPS fix quality (PGN `0xD6`, see §2.5), sent to each client whose DDOP declares DDI 514 as settable. Falls back to `1` when no fresh fix quality is available. | The TC also receives all ISOBUS Process Data (PGN 0xCB00) and Section Control commands from connected implements. diff --git a/include/app.hpp b/include/app.hpp index d74de11..8bc4ce5 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -101,6 +101,9 @@ class Application std::uint32_t lastTCStatusTransmit = 0; std::int32_t lastSpeedValue = 0; std::int32_t lastXteValue = 0; + std::uint8_t gnssFixQuality = 0; ///< AOG fix quality (NMEA 2000 GNSS Method): 0=invalid, 1=GPS, 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated + std::uint32_t lastGnssQualityMs = 0; ///< Timestamp of last PGN 0xD6 fix-quality update (0 = never received) + static constexpr std::uint32_t GNSS_QUALITY_TIMEOUT_MS = 2000; ///< No PGN 0xD6 for this long = fix quality unknown std::uint32_t lastDistanceMm = 0; std::uint32_t lastAogPacketMs = 0; std::uint32_t vtDisconnectedSinceMs = 0; diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 705605e..839a4fc 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -116,6 +116,10 @@ class MyTCServer : public isobus::TaskControllerServer void update_section_states(std::vector §ionStates); void update_section_control_enabled(bool enabled); + /// @brief Sends GNSS quality (DDI 514) to every client that declares that DDI. + /// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated + void send_gnss_quality(std::uint8_t quality); + private: void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); diff --git a/readme.md b/readme.md index 806746c..9eb71b1 100644 --- a/readme.md +++ b/readme.md @@ -91,6 +91,12 @@ The application logs the detected VT version, screen size, softkey dimensions, a - **Maximum Sections:** 64 (supports both individual sections and zone-based control) - **Section Control:** Generation 1 (TC-SC) with support for DDI 160/161/290 +### Guidance data sent to implements + +Sent to any implement whose device description declares the DDI as settable. + +- **GNSS quality (DDI 514):** AgOpenGPS's GPS fix quality, every 250 ms. + ## Contributing Before committing it's better to run these commands: (requires the LLVM project to be installed) diff --git a/src/app.cpp b/src/app.cpp index be27cb6..93e7cbb 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -451,6 +451,39 @@ void Application::setup_udp_connections() static std::uint32_t lastXteTransmit = 0; auto packetHandler = [this](std::uint8_t src, std::uint8_t pgn, std::span data) { + // PGN 0xD6 (214) — GPS/IMU data from AOG (src=0x7C, frame [0x80,0x81,0x7C,0xD6,...]). + // Only the fix-quality byte is consumed today; the rest of the frame (position, + // heading, speed, etc.) is not yet parsed by this TC. + static constexpr std::size_t GNSS_FIX_QUALITY_OFFSET = 38; // frame byte 43 + static constexpr std::size_t MIN_0xD6_PAYLOAD_SIZE = GNSS_FIX_QUALITY_OFFSET + 1; + static constexpr std::uint8_t GNSS_QUALITY_MAX = 8; // SimulateMode + if (src == 0x7C && pgn == 0xD6) + { + static std::uint8_t lastLoggedQuality = 0xFF; + if (data.size() < MIN_0xD6_PAYLOAD_SIZE) + { + std::cout << "[" << get_timestamp() << "] [AOG] PGN 0xD6 received but too short for fix quality (len=" + << data.size() << ")" << std::endl; + return; + } + + std::uint8_t quality = data[GNSS_FIX_QUALITY_OFFSET]; + // AOG fix values follow the NMEA 2000 GNSS Method enumeration that DDI 514 uses: 0=invalid, 1=GPS, + // 2=DGPS, 3=PPS, 4=RTK Fix, 5=Float, 6=Estimated, 7=Manual, 8=Simulated. Forward 0-8 unchanged so + // the implement sees the real source; anything above 8 is not a defined value, so map it to the + // weakest real fix (1=GNSS) rather than 0=No GPS, which would falsely claim there is no position. + gnssFixQuality = (quality <= GNSS_QUALITY_MAX) ? quality : 1; + lastGnssQualityMs = isobus::SystemTiming::get_timestamp_ms(); + + if (quality != lastLoggedQuality) + { + std::cout << "[" << get_timestamp() << "] [GNSS] fix quality byte=" << static_cast(quality) + << " -> DDI514=" << static_cast(gnssFixQuality) << std::endl; + lastLoggedQuality = quality; + } + return; + } + if (src != 0x7F) { return; @@ -719,6 +752,21 @@ bool Application::update() } } + // Send GNSS quality (DDI 514) to implements every 250 ms + static std::uint32_t lastGnssSendMs = 0; + if (tcServer && isobus::SystemTiming::time_expired_ms(lastGnssSendMs, 250)) + { + // PGN 0xD6 is an independently-timed stream — treat the fix quality as unknown once it + // goes stale. Fall back to 1 (weakest real GNSS fix), not 0 (No GNSS): some implements gate + // TRACK/section control on GNSS quality being non-zero, and AOG doesn't always send 0xD6 at + // all (e.g. in Simulator mode) — 0 would falsely claim there is no position fix at all and + // can get commands rejected. + const bool gnssQualityFresh = (lastGnssQualityMs != 0) && + !isobus::SystemTiming::time_expired_ms(lastGnssQualityMs, GNSS_QUALITY_TIMEOUT_MS); + tcServer->send_gnss_quality(gnssQualityFresh ? gnssFixQuality : 1); + lastGnssSendMs = isobus::SystemTiming::get_timestamp_ms(); + } + return true; } diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 660da73..0bf80f0 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -34,6 +34,37 @@ static std::string sanitize_filename(const std::string &input) return result; } +// Finds the number of the first device element that lists the process data object as a child. +static bool find_owning_element_number(isobus::DeviceDescriptorObjectPool &pool, + const isobus::task_controller_object::DeviceProcessDataObject &processData, + std::uint16_t &elementNumber) +{ + for (std::uint32_t i = 0; i < pool.size(); i++) + { + auto object = pool.get_object_by_index(i); + if (!object || object->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceElement) + { + continue; + } + + auto elementObject = std::dynamic_pointer_cast(object); + if (!elementObject) + { + continue; + } + + for (std::uint16_t childId : elementObject->get_child_object_ids()) + { + if (childId == processData.get_object_id()) + { + elementNumber = elementObject->get_element_number(); + return true; + } + } + } + return false; +} + void ClientState::set_number_of_sections(std::uint8_t number) { numberOfSections = number; @@ -812,6 +843,37 @@ void MyTCServer::request_measurement_commands() } } + // Map GNSS quality (DDI 514) so send_gnss_quality() knows which element to address. + // Mapping only: this is a value the TC pushes to the implement, not one to subscribe to. + for (std::uint32_t i = 0; i < client.second.get_pool().size(); i++) + { + auto object = client.second.get_pool().get_object_by_index(i); + if (!object || object->get_object_type() != isobus::task_controller_object::ObjectTypes::DeviceProcessData) + { + continue; + } + + auto processDataObject = std::dynamic_pointer_cast(object); + if (!processDataObject || processDataObject->get_ddi() != static_cast(isobus::DataDescriptionIndex::GNSSQuality)) + { + continue; + } + + if (!processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable)) + { + std::cout << "DDI " << processDataObject->get_ddi() << " (GNSS Quality) is declared but not settable, so it is not sent" << std::endl; + continue; + } + + std::uint16_t elementNumber = 0; + if (find_owning_element_number(client.second.get_pool(), *processDataObject, elementNumber)) + { + client.second.set_element_number_for_ddi(isobus::DataDescriptionIndex::GNSSQuality, elementNumber); + std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (GNSS Quality) to element " + << elementNumber << std::endl; + } + } + std::cout << "[" << get_timestamp() << "] Measurement commands sent." << std::endl; client.second.mark_measurement_commands_sent(); } @@ -884,6 +946,19 @@ void MyTCServer::update_section_control_enabled(bool enabled) } } +void MyTCServer::send_gnss_quality(std::uint8_t quality) +{ + std::lock_guard lock(clientsMutex); + constexpr auto DDI = isobus::DataDescriptionIndex::GNSSQuality; + for (auto &client : clients) + { + if (client.second.has_element_number_for_ddi(DDI)) + { + send_set_value(client.first, static_cast(DDI), client.second.get_element_number_for_ddi(DDI), static_cast(quality)); + } + } +} + void MyTCServer::send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset) { std::lock_guard lock(clientsMutex);