From e281859cdc95135ab6bacf95e4a9da6c20f16289 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 21:35:01 +0100 Subject: [PATCH 1/9] sf3000 to AOG as written by AI --- include/app.hpp | 2 + include/gnss_receiver.hpp | 72 ++++++++++ include/udp_connections.hpp | 8 ++ src/app.cpp | 6 + src/gnss_receiver.cpp | 276 ++++++++++++++++++++++++++++++++++++ src/udp_connections.cpp | 19 +++ 6 files changed, 383 insertions(+) create mode 100644 include/gnss_receiver.hpp create mode 100644 src/gnss_receiver.cpp diff --git a/include/app.hpp b/include/app.hpp index e20086c..f3c1fd8 100644 --- a/include/app.hpp +++ b/include/app.hpp @@ -15,6 +15,7 @@ #include "isobus/isobus/isobus_speed_distance_messages.hpp" #include "isobus/isobus/nmea2000_message_interface.hpp" +#include "gnss_receiver.hpp" #include "settings.hpp" #include "task_controller.hpp" #include "udp_connections.hpp" @@ -38,6 +39,7 @@ class Application std::shared_ptr tecuCF = nullptr; std::unique_ptr speedMessagesInterface; std::unique_ptr nmea2000MessageInterface; + std::unique_ptr gnssReceiver; std::uint8_t nmea2000SequenceIdentifier = 0; std::uint32_t lastJ1939SpeedTransmit = 0; std::int32_t lastSpeedValue = 0; diff --git a/include/gnss_receiver.hpp b/include/gnss_receiver.hpp new file mode 100644 index 0000000..e0fc0a3 --- /dev/null +++ b/include/gnss_receiver.hpp @@ -0,0 +1,72 @@ +/** + * @author Qoder + * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences + * @version 0.1 + * @date 2025-03-14 + */ + +#pragma once + +#include +#include +#include +#include + +#include "isobus/hardware_integration/can_hardware_interface.hpp" +#include "isobus/isobus/can_message_frame.hpp" +#include "isobus/utility/event_dispatcher.hpp" + +#include "udp_connections.hpp" + +class GnssReceiver +{ +public: + GnssReceiver(); + ~GnssReceiver(); + + /// @brief Register the CAN frame listener. Must be called after CANHardwareInterface::start(). + void initialize(); + + /// @brief Build and send a $PANDA sentence if position and time data are available. Throttled to 10 Hz. + /// @param udp The UDP connections to send the sentence on + void send_panda_if_ready(std::shared_ptr udp); + +private: + struct GnssData + { + double latitude_deg = 0.0; + double longitude_deg = 0.0; + double altitude_m = 0.0; + double heading_deg = 0.0; + double speed_kmh = 0.0; + + std::uint8_t hour = 0; + std::uint8_t minute = 0; + std::uint16_t minute_ms = 0; ///< Milliseconds within the current minute + + bool has_position = false; + bool has_time = false; + bool has_altitude = false; + bool has_heading = false; + bool has_speed = false; + }; + + void on_can_frame(const isobus::CANMessageFrame &frame); + void parse_position(const isobus::CANMessageFrame &frame); + void parse_time_date(const isobus::CANMessageFrame &frame); + void parse_altitude(const isobus::CANMessageFrame &frame); + void parse_heading(const isobus::CANMessageFrame &frame); + void parse_speed(const isobus::CANMessageFrame &frame); + + /// @brief Build the $PANDA sentence from the current GNSS data snapshot + /// @param snapshot A copy of the current GNSS data (no lock needed) + /// @return The complete $PANDA sentence including checksum and CRLF + std::string build_panda(const GnssData &snapshot) const; + + static constexpr std::uint8_t SF3000_SOURCE_ADDRESS = 0x1C; + + GnssData data; + std::mutex dataMutex; + isobus::EventCallbackHandle canFrameHandle = 0; + std::uint32_t lastPandaSend = 0; +}; diff --git a/include/udp_connections.hpp b/include/udp_connections.hpp index 001de21..ea88961 100644 --- a/include/udp_connections.hpp +++ b/include/udp_connections.hpp @@ -11,6 +11,7 @@ #include #include +#include #include "settings.hpp" using boost::asio::ip::udp; @@ -69,6 +70,13 @@ class UdpConnections */ bool send(std::uint8_t src, std::uint8_t pgn, std::span data); + /** + * @brief Send raw text to AOG (no binary framing) + * @param text The raw text to send (e.g. an NMEA sentence) + * @return True if the text was sent successfully, false otherwise + */ + bool send_raw(std::string_view text); + private: static const std::size_t MAX_PACKET_SIZE = 512; // Mostly arbitrary, but should be large enough to hold any packet static const std::uint16_t PACKET_START = 0x8081; // Start of packet diff --git a/src/app.cpp b/src/app.cpp index e11ba02..222338f 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -223,6 +223,9 @@ bool Application::initialize() udpConnections->set_packet_handler(packetHandler); udpConnections->open(); + gnssReceiver = std::make_unique(); + gnssReceiver->initialize(); + std::cout << "UDP connections opened." << std::endl; return true; @@ -236,6 +239,9 @@ bool Application::update() udpConnections->handle_address_detection(); udpConnections->handle_incoming_packets(); + if (gnssReceiver) + gnssReceiver->send_panda_if_ready(udpConnections); + tcServer->request_measurement_commands(); tcServer->update(); if (speedMessagesInterface) diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp new file mode 100644 index 0000000..f1a8c34 --- /dev/null +++ b/src/gnss_receiver.cpp @@ -0,0 +1,276 @@ +/** + * @author Qoder + * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences + * @version 0.1 + * @date 2025-03-14 + */ + +#include "gnss_receiver.hpp" + +#include +#include +#include + +#include "isobus/utility/system_timing.hpp" + +GnssReceiver::GnssReceiver() +{ +} + +GnssReceiver::~GnssReceiver() +{ + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher() + .remove_listener(canFrameHandle); +} + +void GnssReceiver::initialize() +{ + canFrameHandle = isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher() + .add_listener([this](const isobus::CANMessageFrame &frame) { + on_can_frame(frame); + }); + std::cout << "[GnssReceiver] CAN frame listener registered for SF3000 (SA=0x" + << std::hex << static_cast(SF3000_SOURCE_ADDRESS) << std::dec << ")" << std::endl; +} + +void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) +{ + if (!frame.isExtendedFrame) + return; + + std::uint8_t sa = frame.identifier & 0xFF; + std::uint8_t pf = (frame.identifier >> 16) & 0xFF; + + // Extract PGN from 29-bit CAN identifier + std::uint32_t pgn; + if (pf >= 0xF0) + { + // PDU2 format: PS byte is group extension, part of PGN + pgn = (frame.identifier >> 8) & 0x3FFFF; + } + else + { + // PDU1 format: PS byte is destination address, not part of PGN + pgn = (frame.identifier >> 8) & 0x3FF00; + } + + // SF3000-specific PGNs (gate on source address) + if (sa == SF3000_SOURCE_ADDRESS) + { + switch (pgn) + { + case 0xFEF3: + parse_position(frame); + break; + case 0xFEF0: + parse_time_date(frame); + break; + case 0xFEF2: + parse_altitude(frame); + break; + case 0xFE45: + parse_heading(frame); + break; + default: + break; + } + } + + // Speed PGN from any source address + if (pgn == 0xFEF1) + { + parse_speed(frame); + } +} + +void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 8) + return; + + auto raw_lat = static_cast( + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24)); + auto raw_lon = static_cast( + static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | + (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24)); + + // 0x7FFFFFFF = "not available" in J1939 + if (raw_lat == 0x7FFFFFFF || raw_lon == 0x7FFFFFFF) + return; + + double lat = raw_lat * 1e-7; + double lon = raw_lon * 1e-7; + + if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) + return; + + std::lock_guard lock(dataMutex); + data.latitude_deg = lat; + data.longitude_deg = lon; + data.has_position = true; +} + +void GnssReceiver::parse_time_date(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 4) + return; + + std::uint16_t ms_of_minute = static_cast(frame.data[0] | (frame.data[1] << 8)); + std::uint8_t min = frame.data[2]; + std::uint8_t hr = frame.data[3]; + + if (hr > 23 || min > 59 || ms_of_minute > 59999) + return; + + std::lock_guard lock(dataMutex); + data.hour = hr; + data.minute = min; + data.minute_ms = ms_of_minute; + data.has_time = true; +} + +void GnssReceiver::parse_altitude(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 4) + return; + + std::uint32_t raw_alt = + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + + if (raw_alt == 0xFFFFFFFF) + return; + + std::lock_guard lock(dataMutex); + data.altitude_m = raw_alt * 0.01; + data.has_altitude = true; +} + +void GnssReceiver::parse_heading(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 2) + return; + + std::uint16_t raw_heading = static_cast(frame.data[0] | (frame.data[1] << 8)); + + if (raw_heading == 0xFFFF) + return; + + std::lock_guard lock(dataMutex); + data.heading_deg = raw_heading * 0.0078125; // 1/128 degree per bit + data.has_heading = true; +} + +void GnssReceiver::parse_speed(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 3) + return; + + // Bytes 1-2 (0-indexed) contain wheel-based vehicle speed + std::uint16_t raw_speed = static_cast(frame.data[1] | (frame.data[2] << 8)); + + if (raw_speed == 0xFFFF) + return; + + std::lock_guard lock(dataMutex); + data.speed_kmh = raw_speed / 256.0; + data.has_speed = true; +} + +void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) +{ + if (!isobus::SystemTiming::time_expired_ms(lastPandaSend, 100)) + return; + + GnssData snapshot; + { + std::lock_guard lock(dataMutex); + snapshot = data; + } + + if (!snapshot.has_position || !snapshot.has_time) + return; + + std::string sentence = build_panda(snapshot); + udp->send_raw(sentence); + lastPandaSend = isobus::SystemTiming::get_timestamp_ms(); +} + +std::string GnssReceiver::build_panda(const GnssData &snapshot) const +{ + // --- Time: HHMMSS.CC --- + std::uint8_t ss = (snapshot.minute_ms / 1000) % 60; + std::uint8_t cc = (snapshot.minute_ms % 1000) / 10; + char timeStr[12]; + std::snprintf(timeStr, sizeof(timeStr), "%02d%02d%02d.%02d", + snapshot.hour, snapshot.minute, ss, cc); + + // --- Latitude: DDMM.MMMM + N/S --- + double absLat = std::fabs(snapshot.latitude_deg); + int latDeg = static_cast(absLat); + double latMin = (absLat - latDeg) * 60.0; + char latStr[16]; + std::snprintf(latStr, sizeof(latStr), "%02d%07.4f", latDeg, latMin); + char latHemi = snapshot.latitude_deg >= 0.0 ? 'N' : 'S'; + + // --- Longitude: DDDMM.MMMM + E/W --- + double absLon = std::fabs(snapshot.longitude_deg); + int lonDeg = static_cast(absLon); + double lonMin = (absLon - lonDeg) * 60.0; + char lonStr[16]; + std::snprintf(lonStr, sizeof(lonStr), "%03d%07.4f", lonDeg, lonMin); + char lonHemi = snapshot.longitude_deg >= 0.0 ? 'E' : 'W'; + + // --- Altitude --- + char altStr[16]; + std::snprintf(altStr, sizeof(altStr), "%.2f", snapshot.has_altitude ? snapshot.altitude_m : 0.0); + + // --- Speed (km/h -> knots) --- + double speedKnots = snapshot.has_speed ? snapshot.speed_kmh / 1.852 : 0.0; + char speedStr[16]; + std::snprintf(speedStr, sizeof(speedStr), "%.2f", speedKnots); + + // --- Heading --- + char headingStr[16]; + std::snprintf(headingStr, sizeof(headingStr), "%.1f", snapshot.has_heading ? snapshot.heading_deg : 0.0); + + // --- Assemble body (everything between $ and *) --- + // $PANDA,time,lat,N,lon,E,fix,sats,hdop,alt,ageDGPS,speedKnots,heading,roll,pitch,yawRate*CS + char body[256]; + std::snprintf(body, sizeof(body), + "PANDA,%s,%s,%c,%s,%c,%d,%d,%.1f,%s,%.1f,%s,%s,%.1f,%.1f,%.1f", + timeStr, + latStr, latHemi, + lonStr, lonHemi, + 4, // fixType (RTK default) + 18, // satellites default + 0.8, // HDOP default + altStr, + 0.0, // ageDGPS + speedStr, + headingStr, + 0.0, // roll + 0.0, // pitch + 0.0 // yawRate + ); + + // --- Checksum: XOR of all chars in body --- + std::uint8_t cs = 0; + for (const char *p = body; *p != '\0'; p++) + { + cs ^= static_cast(*p); + } + + char checksum[4]; + std::snprintf(checksum, sizeof(checksum), "%02X", cs); + + // --- Final sentence --- + std::string sentence = "$"; + sentence += body; + sentence += "*"; + sentence += checksum; + sentence += "\r\n"; + + return sentence; +} diff --git a/src/udp_connections.cpp b/src/udp_connections.cpp index f7de3d6..7ddfbe2 100644 --- a/src/udp_connections.cpp +++ b/src/udp_connections.cpp @@ -280,3 +280,22 @@ bool UdpConnections::send(std::uint8_t src, std::uint8_t pgn, std::spanget_subnet(); + boost::asio::ip::address_v4 broadcast_address = boost::asio::ip::make_address_v4(std::to_string(subnet[0]) + "." + + std::to_string(subnet[1]) + "." + + std::to_string(subnet[2]) + ".255"); + + udp::endpoint broadcast_endpoint(broadcast_address, 9999); + try + { + udpConnection.send_to(boost::asio::buffer(text.data(), text.size()), broadcast_endpoint); + } + catch (const boost::system::system_error &e) + { + return false; + } + return true; +} From a40f4dd8399746a7df93a5be155c20df58e6feab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 21:48:23 +0100 Subject: [PATCH 2/9] more logs --- include/gnss_receiver.hpp | 3 +++ src/gnss_receiver.cpp | 56 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+) diff --git a/include/gnss_receiver.hpp b/include/gnss_receiver.hpp index e0fc0a3..374eacc 100644 --- a/include/gnss_receiver.hpp +++ b/include/gnss_receiver.hpp @@ -69,4 +69,7 @@ class GnssReceiver std::mutex dataMutex; isobus::EventCallbackHandle canFrameHandle = 0; std::uint32_t lastPandaSend = 0; + std::uint32_t lastPandaLog = 0; + std::uint32_t lastWaitingLog = 0; + bool pandaSending = false; }; diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index f1a8c34..961e5d9 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -103,9 +103,16 @@ void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) double lon = raw_lon * 1e-7; if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) + { + std::cout << "[GnssReceiver] PGN 0xFEF3 position out of range: lat=" << lat << " lon=" << lon << std::endl; return; + } std::lock_guard lock(dataMutex); + if (!data.has_position) + { + std::cout << "[GnssReceiver] First position fix received: lat=" << lat << " lon=" << lon << std::endl; + } data.latitude_deg = lat; data.longitude_deg = lon; data.has_position = true; @@ -124,6 +131,11 @@ void GnssReceiver::parse_time_date(const isobus::CANMessageFrame &frame) return; std::lock_guard lock(dataMutex); + if (!data.has_time) + { + std::cout << "[GnssReceiver] First time/date received: " + << static_cast(hr) << ":" << static_cast(min) << " (" << ms_of_minute << "ms)" << std::endl; + } data.hour = hr; data.minute = min; data.minute_ms = ms_of_minute; @@ -143,6 +155,10 @@ void GnssReceiver::parse_altitude(const isobus::CANMessageFrame &frame) return; std::lock_guard lock(dataMutex); + if (!data.has_altitude) + { + std::cout << "[GnssReceiver] First altitude received: " << (raw_alt * 0.01) << " m" << std::endl; + } data.altitude_m = raw_alt * 0.01; data.has_altitude = true; } @@ -158,6 +174,10 @@ void GnssReceiver::parse_heading(const isobus::CANMessageFrame &frame) return; std::lock_guard lock(dataMutex); + if (!data.has_heading) + { + std::cout << "[GnssReceiver] First heading received: " << (raw_heading * 0.0078125) << " deg" << std::endl; + } data.heading_deg = raw_heading * 0.0078125; // 1/128 degree per bit data.has_heading = true; } @@ -174,6 +194,11 @@ void GnssReceiver::parse_speed(const isobus::CANMessageFrame &frame) return; std::lock_guard lock(dataMutex); + if (!data.has_speed) + { + std::cout << "[GnssReceiver] First speed received: " << (raw_speed / 256.0) << " km/h (SA=0x" + << std::hex << static_cast(frame.identifier & 0xFF) << std::dec << ")" << std::endl; + } data.speed_kmh = raw_speed / 256.0; data.has_speed = true; } @@ -190,9 +215,40 @@ void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) } if (!snapshot.has_position || !snapshot.has_time) + { + if (isobus::SystemTiming::time_expired_ms(lastWaitingLog, 5000)) + { + std::cout << "[GnssReceiver] Waiting for GNSS data... position=" + << (snapshot.has_position ? "OK" : "MISSING") + << " time=" << (snapshot.has_time ? "OK" : "MISSING") + << " altitude=" << (snapshot.has_altitude ? "OK" : "no") + << " heading=" << (snapshot.has_heading ? "OK" : "no") + << " speed=" << (snapshot.has_speed ? "OK" : "no") + << std::endl; + lastWaitingLog = isobus::SystemTiming::get_timestamp_ms(); + } return; + } std::string sentence = build_panda(snapshot); + + if (!pandaSending) + { + std::cout << "[GnssReceiver] Sending first $PANDA sentence: " << sentence.substr(0, sentence.size() - 2) << std::endl; + pandaSending = true; + } + + if (isobus::SystemTiming::time_expired_ms(lastPandaLog, 10000)) + { + std::cout << "[GnssReceiver] $PANDA: lat=" << snapshot.latitude_deg + << " lon=" << snapshot.longitude_deg + << " alt=" << snapshot.altitude_m + << " hdg=" << snapshot.heading_deg + << " spd=" << snapshot.speed_kmh << "km/h" + << std::endl; + lastPandaLog = isobus::SystemTiming::get_timestamp_ms(); + } + udp->send_raw(sentence); lastPandaSend = isobus::SystemTiming::get_timestamp_ms(); } From 5b22c4b3600b8b1377c1651fc8fce8d1929bb35a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:00:12 +0100 Subject: [PATCH 3/9] fix position? --- src/gnss_receiver.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index 961e5d9..9ef7a35 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -88,19 +88,20 @@ void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) if (frame.dataLength < 8) return; - auto raw_lat = static_cast( + // J1939 PGN 65267: latitude/longitude are uint32, resolution 1e-7 deg/bit, offset -210 deg + std::uint32_t raw_lat = static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | - (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24)); - auto raw_lon = static_cast( + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + std::uint32_t raw_lon = static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | - (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24)); + (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24); - // 0x7FFFFFFF = "not available" in J1939 - if (raw_lat == 0x7FFFFFFF || raw_lon == 0x7FFFFFFF) + // 0xFFFFFFFF = "not available" in J1939 for unsigned parameters + if (raw_lat == 0xFFFFFFFF || raw_lon == 0xFFFFFFFF) return; - double lat = raw_lat * 1e-7; - double lon = raw_lon * 1e-7; + double lat = raw_lat * 1e-7 - 210.0; + double lon = raw_lon * 1e-7 - 210.0; if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) { From 9ff70ce3a2412e4f0fa02969a6ae7284337afc1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:10:59 +0100 Subject: [PATCH 4/9] a --- src/gnss_receiver.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index 9ef7a35..f035ebb 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -215,17 +215,11 @@ void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) snapshot = data; } - if (!snapshot.has_position || !snapshot.has_time) + if (!snapshot.has_position) { if (isobus::SystemTiming::time_expired_ms(lastWaitingLog, 5000)) { - std::cout << "[GnssReceiver] Waiting for GNSS data... position=" - << (snapshot.has_position ? "OK" : "MISSING") - << " time=" << (snapshot.has_time ? "OK" : "MISSING") - << " altitude=" << (snapshot.has_altitude ? "OK" : "no") - << " heading=" << (snapshot.has_heading ? "OK" : "no") - << " speed=" << (snapshot.has_speed ? "OK" : "no") - << std::endl; + std::cout << "[GnssReceiver] Waiting for GNSS position fix..." << std::endl; lastWaitingLog = isobus::SystemTiming::get_timestamp_ms(); } return; From e790fbaded6867d907ec83dbd585ef7da7fc4159 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:18:18 +0100 Subject: [PATCH 5/9] =?UTF-8?q?maybe=20now=3F=C3=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/gnss_receiver.hpp | 60 ++++-- src/app.cpp | 2 +- src/gnss_receiver.cpp | 435 ++++++++++++++++++++++++++++++++++---- 3 files changed, 444 insertions(+), 53 deletions(-) diff --git a/include/gnss_receiver.hpp b/include/gnss_receiver.hpp index 374eacc..6e3968a 100644 --- a/include/gnss_receiver.hpp +++ b/include/gnss_receiver.hpp @@ -1,16 +1,18 @@ /** * @author Qoder * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences - * @version 0.1 + * @version 0.2 * @date 2025-03-14 */ #pragma once #include +#include #include #include #include +#include #include "isobus/hardware_integration/can_hardware_interface.hpp" #include "isobus/isobus/can_message_frame.hpp" @@ -25,9 +27,10 @@ class GnssReceiver ~GnssReceiver(); /// @brief Register the CAN frame listener. Must be called after CANHardwareInterface::start(). - void initialize(); + /// @param reverseEngineerMode If true, log all SF3000 frames to CSV for offline analysis + void initialize(bool reverseEngineerMode = false); - /// @brief Build and send a $PANDA sentence if position and time data are available. Throttled to 10 Hz. + /// @brief Build and send a $PANDA sentence if position data is available. Throttled to 10 Hz. /// @param udp The UDP connections to send the sentence on void send_panda_if_ready(std::shared_ptr udp); @@ -42,7 +45,7 @@ class GnssReceiver std::uint8_t hour = 0; std::uint8_t minute = 0; - std::uint16_t minute_ms = 0; ///< Milliseconds within the current minute + std::uint16_t minute_ms = 0; bool has_position = false; bool has_time = false; @@ -51,19 +54,42 @@ class GnssReceiver bool has_speed = false; }; + /// @brief Per-PGN tracking for reverse engineering + struct PgnTracker + { + std::uint32_t count = 0; + std::uint8_t lastPayload[8] = {}; + std::uint8_t lastLength = 0; + std::uint32_t firstSeenMs = 0; + std::uint32_t lastSeenMs = 0; + }; + void on_can_frame(const isobus::CANMessageFrame &frame); - void parse_position(const isobus::CANMessageFrame &frame); - void parse_time_date(const isobus::CANMessageFrame &frame); - void parse_altitude(const isobus::CANMessageFrame &frame); - void parse_heading(const isobus::CANMessageFrame &frame); - void parse_speed(const isobus::CANMessageFrame &frame); - - /// @brief Build the $PANDA sentence from the current GNSS data snapshot - /// @param snapshot A copy of the current GNSS data (no lock needed) - /// @return The complete $PANDA sentence including checksum and CRLF + + // Standard J1939 parsers (kept for buses that broadcast them) + void parse_position_standard(const isobus::CANMessageFrame &frame); + void parse_time_date_standard(const isobus::CANMessageFrame &frame); + void parse_altitude_standard(const isobus::CANMessageFrame &frame); + void parse_speed_standard(const isobus::CANMessageFrame &frame); + + // Confirmed/candidate parsers for proprietary Deere PGNs + void parse_heading_FE45(const isobus::CANMessageFrame &frame); + void parse_candidate_FE43(const isobus::CANMessageFrame &frame); + void parse_candidate_FE12(const isobus::CANMessageFrame &frame); + void parse_candidate_FE13(const isobus::CANMessageFrame &frame); + void parse_candidate_FFFA(const isobus::CANMessageFrame &frame); + void parse_candidate_FFFB(const isobus::CANMessageFrame &frame); + void parse_candidate_ACFF(const isobus::CANMessageFrame &frame); + void parse_candidate_FE0A(const isobus::CANMessageFrame &frame); + void parse_candidate_F022(const isobus::CANMessageFrame &frame); + void parse_candidate_FFFF(const isobus::CANMessageFrame &frame); + + void log_frame_raw(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame); + void log_frame_csv(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame); + std::string build_panda(const GnssData &snapshot) const; - static constexpr std::uint8_t SF3000_SOURCE_ADDRESS = 0x1C; + static constexpr std::uint8_t SF3000_SOURCE_ADDRESS = 0x9A; GnssData data; std::mutex dataMutex; @@ -71,5 +97,11 @@ class GnssReceiver std::uint32_t lastPandaSend = 0; std::uint32_t lastPandaLog = 0; std::uint32_t lastWaitingLog = 0; + std::uint32_t lastPgnSummaryLog = 0; bool pandaSending = false; + bool reMode = false; + + std::unordered_map pgnTrackers; + std::mutex trackerMutex; + std::ofstream csvFile; }; diff --git a/src/app.cpp b/src/app.cpp index 222338f..7d45112 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -224,7 +224,7 @@ bool Application::initialize() udpConnections->open(); gnssReceiver = std::make_unique(); - gnssReceiver->initialize(); + gnssReceiver->initialize(true); // RE mode: log all SF3000 frames to CSV std::cout << "UDP connections opened." << std::endl; diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index f035ebb..6559675 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -1,7 +1,7 @@ /** * @author Qoder * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences - * @version 0.1 + * @version 0.2 * @date 2025-03-14 */ @@ -9,7 +9,9 @@ #include #include +#include #include +#include #include "isobus/utility/system_timing.hpp" @@ -21,10 +23,28 @@ GnssReceiver::~GnssReceiver() { isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher() .remove_listener(canFrameHandle); + if (csvFile.is_open()) + { + csvFile.close(); + } } -void GnssReceiver::initialize() +void GnssReceiver::initialize(bool reverseEngineerMode) { + reMode = reverseEngineerMode; + + if (reMode) + { + csvFile.open("sf3000_can_log.csv", std::ios::out | std::ios::trunc); + if (csvFile.is_open()) + { + csvFile << "timestamp_ms,can_id,pgn,sa,dlc,b0,b1,b2,b3,b4,b5,b6,b7" << std::endl; + std::cout << "[GnssReceiver] RE mode: logging all SA=0x" + << std::hex << static_cast(SF3000_SOURCE_ADDRESS) << std::dec + << " frames to sf3000_can_log.csv" << std::endl; + } + } + canFrameHandle = isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher() .add_listener([this](const isobus::CANMessageFrame &frame) { on_can_frame(frame); @@ -33,6 +53,41 @@ void GnssReceiver::initialize() << std::hex << static_cast(SF3000_SOURCE_ADDRESS) << std::dec << ")" << std::endl; } +void GnssReceiver::log_frame_raw(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame) +{ + std::ostringstream oss; + oss << "[GnssReceiver] CAN ID=0x" << std::hex << std::uppercase << frame.identifier + << " PGN=0x" << pgn << " SA=0x" << static_cast(sa) + << " DLC=" << std::dec << static_cast(frame.dataLength) << " DATA=["; + for (int i = 0; i < frame.dataLength; i++) + { + if (i > 0) + oss << " "; + oss << std::hex << std::setw(2) << std::setfill('0') << std::uppercase << static_cast(frame.data[i]); + } + oss << "]" << std::dec; + std::cout << oss.str() << std::endl; +} + +void GnssReceiver::log_frame_csv(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame) +{ + if (!csvFile.is_open()) + return; + + csvFile << isobus::SystemTiming::get_timestamp_ms() << "," + << "0x" << std::hex << std::uppercase << frame.identifier << "," + << "0x" << pgn << "," + << "0x" << static_cast(sa) << std::dec << "," + << static_cast(frame.dataLength); + for (int i = 0; i < 8; i++) + { + csvFile << ","; + if (i < frame.dataLength) + csvFile << "0x" << std::hex << std::setw(2) << std::setfill('0') << std::uppercase << static_cast(frame.data[i]) << std::dec; + } + csvFile << std::endl; +} + void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) { if (!frame.isExtendedFrame) @@ -41,54 +96,120 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) std::uint8_t sa = frame.identifier & 0xFF; std::uint8_t pf = (frame.identifier >> 16) & 0xFF; - // Extract PGN from 29-bit CAN identifier std::uint32_t pgn; if (pf >= 0xF0) { - // PDU2 format: PS byte is group extension, part of PGN pgn = (frame.identifier >> 8) & 0x3FFFF; } else { - // PDU1 format: PS byte is destination address, not part of PGN pgn = (frame.identifier >> 8) & 0x3FF00; } - // SF3000-specific PGNs (gate on source address) + // Track all frames from SF3000 if (sa == SF3000_SOURCE_ADDRESS) { + // Update PGN tracker + { + std::lock_guard lock(trackerMutex); + auto &tracker = pgnTrackers[pgn]; + if (tracker.count == 0) + { + tracker.firstSeenMs = isobus::SystemTiming::get_timestamp_ms(); + std::cout << "[GnssReceiver] NEW PGN discovered from SF3000: 0x" + << std::hex << std::uppercase << pgn << std::dec + << " (CAN ID=0x" << std::hex << frame.identifier << std::dec << ")" << std::endl; + // Log first occurrence payload in detail + log_frame_raw(pgn, sa, frame); + } + tracker.count++; + tracker.lastSeenMs = isobus::SystemTiming::get_timestamp_ms(); + tracker.lastLength = frame.dataLength; + for (int i = 0; i < frame.dataLength && i < 8; i++) + { + tracker.lastPayload[i] = frame.data[i]; + } + } + + // CSV logging in RE mode + if (reMode) + { + log_frame_csv(pgn, sa, frame); + } + + // Dispatch to parsers for known/candidate PGNs switch (pgn) { + // Standard J1939 PGNs (may or may not be present) case 0xFEF3: - parse_position(frame); + parse_position_standard(frame); break; case 0xFEF0: - parse_time_date(frame); + parse_time_date_standard(frame); break; case 0xFEF2: - parse_altitude(frame); + parse_altitude_standard(frame); break; + + // Confirmed candidate: heading case 0xFE45: - parse_heading(frame); + parse_heading_FE45(frame); + break; + + // Proprietary Deere PGNs - candidate parsers + case 0xFE43: + parse_candidate_FE43(frame); + break; + case 0xFE12: + parse_candidate_FE12(frame); + break; + case 0xFE13: + parse_candidate_FE13(frame); break; + case 0xFE0A: + parse_candidate_FE0A(frame); + break; + case 0xF022: + parse_candidate_F022(frame); + break; + case 0xFFFA: + parse_candidate_FFFA(frame); + break; + case 0xFFFB: + parse_candidate_FFFB(frame); + break; + case 0xFFFF: + parse_candidate_FFFF(frame); + break; + default: break; } } - // Speed PGN from any source address + // Handle ACFF: PDU1 with PF=0xAC, destination=0xFF (global broadcast) + // CAN ID example: 0x0CACFF9A -> PGN extracted as 0xAC00 by PDU1 rules (PS is destination, not part of PGN) + if (sa == SF3000_SOURCE_ADDRESS && pgn == 0xAC00) + { + parse_candidate_ACFF(frame); + } + + // Speed PGN from any source address (standard J1939) if (pgn == 0xFEF1) { - parse_speed(frame); + parse_speed_standard(frame); } } -void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) +// ============================================================================ +// Standard J1939 parsers (kept for buses that may still broadcast them) +// ============================================================================ + +void GnssReceiver::parse_position_standard(const isobus::CANMessageFrame &frame) { if (frame.dataLength < 8) return; - // J1939 PGN 65267: latitude/longitude are uint32, resolution 1e-7 deg/bit, offset -210 deg std::uint32_t raw_lat = static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); @@ -96,7 +217,6 @@ void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24); - // 0xFFFFFFFF = "not available" in J1939 for unsigned parameters if (raw_lat == 0xFFFFFFFF || raw_lon == 0xFFFFFFFF) return; @@ -112,19 +232,20 @@ void GnssReceiver::parse_position(const isobus::CANMessageFrame &frame) std::lock_guard lock(dataMutex); if (!data.has_position) { - std::cout << "[GnssReceiver] First position fix received: lat=" << lat << " lon=" << lon << std::endl; + std::cout << "[GnssReceiver] *** POSITION set by PGN 0xFEF3 *** lat=" << lat << " lon=" << lon << std::endl; } data.latitude_deg = lat; data.longitude_deg = lon; data.has_position = true; } -void GnssReceiver::parse_time_date(const isobus::CANMessageFrame &frame) +void GnssReceiver::parse_time_date_standard(const isobus::CANMessageFrame &frame) { if (frame.dataLength < 4) return; - std::uint16_t ms_of_minute = static_cast(frame.data[0] | (frame.data[1] << 8)); + std::uint16_t ms_of_minute = static_cast( + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8)); std::uint8_t min = frame.data[2]; std::uint8_t hr = frame.data[3]; @@ -134,7 +255,7 @@ void GnssReceiver::parse_time_date(const isobus::CANMessageFrame &frame) std::lock_guard lock(dataMutex); if (!data.has_time) { - std::cout << "[GnssReceiver] First time/date received: " + std::cout << "[GnssReceiver] *** TIME set by PGN 0xFEF0 *** " << static_cast(hr) << ":" << static_cast(min) << " (" << ms_of_minute << "ms)" << std::endl; } data.hour = hr; @@ -143,7 +264,7 @@ void GnssReceiver::parse_time_date(const isobus::CANMessageFrame &frame) data.has_time = true; } -void GnssReceiver::parse_altitude(const isobus::CANMessageFrame &frame) +void GnssReceiver::parse_altitude_standard(const isobus::CANMessageFrame &frame) { if (frame.dataLength < 4) return; @@ -158,57 +279,291 @@ void GnssReceiver::parse_altitude(const isobus::CANMessageFrame &frame) std::lock_guard lock(dataMutex); if (!data.has_altitude) { - std::cout << "[GnssReceiver] First altitude received: " << (raw_alt * 0.01) << " m" << std::endl; + std::cout << "[GnssReceiver] *** ALTITUDE set by PGN 0xFEF2 *** " << (raw_alt * 0.01) << " m" << std::endl; } data.altitude_m = raw_alt * 0.01; data.has_altitude = true; } -void GnssReceiver::parse_heading(const isobus::CANMessageFrame &frame) +void GnssReceiver::parse_speed_standard(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 3) + return; + + std::uint16_t raw_speed = static_cast( + static_cast(frame.data[1]) | (static_cast(frame.data[2]) << 8)); + + if (raw_speed == 0xFFFF) + return; + + std::lock_guard lock(dataMutex); + if (!data.has_speed) + { + std::cout << "[GnssReceiver] *** SPEED set by PGN 0xFEF1 *** " << (raw_speed / 256.0) << " km/h (SA=0x" + << std::hex << static_cast(frame.identifier & 0xFF) << std::dec << ")" << std::endl; + } + data.speed_kmh = raw_speed / 256.0; + data.has_speed = true; +} + +// ============================================================================ +// Confirmed/candidate: PGN 0xFE45 - Heading +// ============================================================================ + +void GnssReceiver::parse_heading_FE45(const isobus::CANMessageFrame &frame) { if (frame.dataLength < 2) return; - std::uint16_t raw_heading = static_cast(frame.data[0] | (frame.data[1] << 8)); + std::uint16_t raw_heading = static_cast( + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8)); if (raw_heading == 0xFFFF) return; + double heading = raw_heading * 0.0078125; // 1/128 degree per bit + std::lock_guard lock(dataMutex); if (!data.has_heading) { - std::cout << "[GnssReceiver] First heading received: " << (raw_heading * 0.0078125) << " deg" << std::endl; + std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading << " deg" << std::endl; } - data.heading_deg = raw_heading * 0.0078125; // 1/128 degree per bit + data.heading_deg = heading; data.has_heading = true; } -void GnssReceiver::parse_speed(const isobus::CANMessageFrame &frame) +// ============================================================================ +// Proprietary Deere candidate parsers +// Each logs decoded candidate fields. Actual field mapping TBD via RE. +// ============================================================================ + +void GnssReceiver::parse_candidate_FE43(const isobus::CANMessageFrame &frame) { - if (frame.dataLength < 3) + // PGN 0xFE43 (65091) - Deere proprietary, possibly vehicle dynamics or tilt + // Expected: near Kecskemet, tilt = -1.2 deg + if (frame.dataLength < 2) return; - // Bytes 1-2 (0-indexed) contain wheel-based vehicle speed - std::uint16_t raw_speed = static_cast(frame.data[1] | (frame.data[2] << 8)); + std::uint16_t w0 = static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8); + double candidate_angle = static_cast(w0) * 0.0078125; // same scale as heading? - if (raw_speed == 0xFFFF) + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFE43 candidate: word0_as_angle=" << candidate_angle + << " word0_raw=" << w0 << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_FE12(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFE12 (65042) - Deere proprietary + if (frame.dataLength < 4) return; - std::lock_guard lock(dataMutex); - if (!data.has_speed) + std::uint32_t dw0 = + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + + // Try as J1939 position (uint32 * 1e-7 - 210) + double as_pos = dw0 * 1e-7 - 210.0; + // Try as altitude (uint32 * 0.01) + double as_alt = dw0 * 0.01; + + static bool logged = false; + if (!logged) { - std::cout << "[GnssReceiver] First speed received: " << (raw_speed / 256.0) << " km/h (SA=0x" - << std::hex << static_cast(frame.identifier & 0xFF) << std::dec << ")" << std::endl; + std::cout << "[GnssReceiver] PGN 0xFE12 candidate: dw0=0x" << std::hex << dw0 << std::dec + << " as_position=" << as_pos + << " as_altitude_cm=" << as_alt << std::endl; + logged = true; } - data.speed_kmh = raw_speed / 256.0; - data.has_speed = true; } +void GnssReceiver::parse_candidate_FE13(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFE13 (65043) - Deere proprietary + if (frame.dataLength < 4) + return; + + std::uint32_t dw0 = + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + + double as_pos = dw0 * 1e-7 - 210.0; + double as_alt = dw0 * 0.01; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFE13 candidate: dw0=0x" << std::hex << dw0 << std::dec + << " as_position=" << as_pos + << " as_altitude_cm=" << as_alt << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_FE0A(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFE0A (65034) - Deere proprietary + if (frame.dataLength < 2) + return; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFE0A candidate: first payload logged at discovery" << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_F022(const isobus::CANMessageFrame &frame) +{ + // PGN 0xF022 (61474) - Deere proprietary, possibly GNSS status + if (frame.dataLength < 2) + return; + + // Byte 0 might be fix type, byte 1 might be satellite count + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xF022 candidate: b0=" << static_cast(frame.data[0]) + << " b1=" << static_cast(frame.data[1]); + if (frame.dataLength >= 4) + { + std::uint16_t w1 = static_cast(frame.data[2]) | (static_cast(frame.data[3]) << 8); + std::cout << " w1=" << w1 << " w1_as_hdop_0.01=" << (w1 * 0.01); + } + std::cout << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_FFFA(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFFFA (65530) - Deere proprietary, possibly GNSS position or extended data + if (frame.dataLength < 8) + return; + + // Try interpreting as two uint32 position fields (same encoding as PGN 0xFEF3) + std::uint32_t dw0 = + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + std::uint32_t dw1 = + static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | + (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24); + + double as_lat = dw0 * 1e-7 - 210.0; + double as_lon = dw1 * 1e-7 - 210.0; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFFFA candidate: dw0=0x" << std::hex << dw0 + << " dw1=0x" << dw1 << std::dec + << " as_lat=" << as_lat + << " as_lon=" << as_lon + << " dw0_as_alt_cm=" << (dw0 * 0.01) + << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_FFFB(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFFFB (65531) - Deere proprietary, possibly GNSS position or time + if (frame.dataLength < 8) + return; + + std::uint32_t dw0 = + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | + (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); + std::uint32_t dw1 = + static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | + (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24); + + double as_lat = dw0 * 1e-7 - 210.0; + double as_lon = dw1 * 1e-7 - 210.0; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFFFB candidate: dw0=0x" << std::hex << dw0 + << " dw1=0x" << dw1 << std::dec + << " as_lat=" << as_lat + << " as_lon=" << as_lon + << " dw0_as_alt_cm=" << (dw0 * 0.01) + << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_ACFF(const isobus::CANMessageFrame &frame) +{ + // PGN 0xACFF / 0xAC00 - Deere proprietary transport/multi-packet or GNSS extended + if (frame.dataLength < 2) + return; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xACFF candidate: first payload logged at discovery" << std::endl; + logged = true; + } +} + +void GnssReceiver::parse_candidate_FFFF(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFFFF (65535) - Deere proprietary + if (frame.dataLength < 2) + return; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFFFF candidate: first payload logged at discovery" << std::endl; + logged = true; + } +} + +// ============================================================================ +// PANDA generation +// ============================================================================ + void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) { if (!isobus::SystemTiming::time_expired_ms(lastPandaSend, 100)) return; + // Periodic PGN summary (every 30s) + if (isobus::SystemTiming::time_expired_ms(lastPgnSummaryLog, 30000)) + { + std::lock_guard lock(trackerMutex); + if (!pgnTrackers.empty()) + { + std::cout << "[GnssReceiver] === PGN Summary (SA=0x" + << std::hex << static_cast(SF3000_SOURCE_ADDRESS) << std::dec << ") ===" << std::endl; + for (auto &[pgn, t] : pgnTrackers) + { + std::uint32_t elapsed = t.lastSeenMs - t.firstSeenMs; + double hz = (elapsed > 0 && t.count > 1) ? ((t.count - 1) * 1000.0 / elapsed) : 0.0; + std::cout << " PGN 0x" << std::hex << std::uppercase << pgn << std::dec + << ": count=" << t.count + << " ~" << std::fixed << std::setprecision(1) << hz << "Hz" + << " last=["; + for (int i = 0; i < t.lastLength && i < 8; i++) + { + if (i > 0) + std::cout << " "; + std::cout << std::hex << std::setw(2) << std::setfill('0') << std::uppercase << static_cast(t.lastPayload[i]); + } + std::cout << "]" << std::dec << std::endl; + } + } + lastPgnSummaryLog = isobus::SystemTiming::get_timestamp_ms(); + } + GnssData snapshot; { std::lock_guard lock(dataMutex); @@ -219,7 +574,12 @@ void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) { if (isobus::SystemTiming::time_expired_ms(lastWaitingLog, 5000)) { - std::cout << "[GnssReceiver] Waiting for GNSS position fix..." << std::endl; + std::cout << "[GnssReceiver] Waiting for GNSS position fix..." + << " time=" << (snapshot.has_time ? "OK" : "no") + << " alt=" << (snapshot.has_altitude ? "OK" : "no") + << " hdg=" << (snapshot.has_heading ? "OK" : "no") + << " spd=" << (snapshot.has_speed ? "OK" : "no") + << std::endl; lastWaitingLog = isobus::SystemTiming::get_timestamp_ms(); } return; @@ -287,7 +647,6 @@ std::string GnssReceiver::build_panda(const GnssData &snapshot) const std::snprintf(headingStr, sizeof(headingStr), "%.1f", snapshot.has_heading ? snapshot.heading_deg : 0.0); // --- Assemble body (everything between $ and *) --- - // $PANDA,time,lat,N,lon,E,fix,sats,hdop,alt,ageDGPS,speedKnots,heading,roll,pitch,yawRate*CS char body[256]; std::snprintf(body, sizeof(body), "PANDA,%s,%s,%c,%s,%c,%d,%d,%.1f,%s,%.1f,%s,%s,%.1f,%.1f,%.1f", From bc2e062536903f7da639ac3586a85c273ce50562 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:28:43 +0100 Subject: [PATCH 6/9] heading parsed? --- src/gnss_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index 6559675..44135dc 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -321,12 +321,13 @@ void GnssReceiver::parse_heading_FE45(const isobus::CANMessageFrame &frame) if (raw_heading == 0xFFFF) return; - double heading = raw_heading * 0.0078125; // 1/128 degree per bit + // Deere proprietary scale: raw / 27.5 degrees (confirmed via RE capture) + double heading = raw_heading / 27.5; std::lock_guard lock(dataMutex); if (!data.has_heading) { - std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading << " deg" << std::endl; + std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading << " deg (raw=" << raw_heading << ")" << std::endl; } data.heading_deg = heading; data.has_heading = true; From 5ee22b945d3dfbbf0c025fc8339bdf7c27ae6d51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:44:49 +0100 Subject: [PATCH 7/9] heading too --- include/gnss_receiver.hpp | 2 +- src/gnss_receiver.cpp | 90 +++++++++++++++++---------------------- 2 files changed, 39 insertions(+), 53 deletions(-) diff --git a/include/gnss_receiver.hpp b/include/gnss_receiver.hpp index 6e3968a..b0348ee 100644 --- a/include/gnss_receiver.hpp +++ b/include/gnss_receiver.hpp @@ -87,7 +87,7 @@ class GnssReceiver void log_frame_raw(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame); void log_frame_csv(std::uint32_t pgn, std::uint8_t sa, const isobus::CANMessageFrame &frame); - std::string build_panda(const GnssData &snapshot) const; + std::string build_gga(const GnssData &snapshot) const; static constexpr std::uint8_t SF3000_SOURCE_ADDRESS = 0x9A; diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index 44135dc..d0f0789 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -137,21 +137,10 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) log_frame_csv(pgn, sa, frame); } - // Dispatch to parsers for known/candidate PGNs + // Dispatch to parsers for SF3000 proprietary PGNs switch (pgn) { - // Standard J1939 PGNs (may or may not be present) - case 0xFEF3: - parse_position_standard(frame); - break; - case 0xFEF0: - parse_time_date_standard(frame); - break; - case 0xFEF2: - parse_altitude_standard(frame); - break; - - // Confirmed candidate: heading + // Confirmed: heading case 0xFE45: parse_heading_FE45(frame); break; @@ -194,10 +183,24 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) parse_candidate_ACFF(frame); } - // Speed PGN from any source address (standard J1939) - if (pgn == 0xFEF1) + // Standard J1939 GNSS PGNs - accept from ANY source address + // (SF3000 at SA=0x9A uses proprietary PGNs, but other ECUs on the bus may broadcast these) + switch (pgn) { - parse_speed_standard(frame); + case 0xFEF3: + parse_position_standard(frame); + break; + case 0xFEF0: + parse_time_date_standard(frame); + break; + case 0xFEF2: + parse_altitude_standard(frame); + break; + case 0xFEF1: + parse_speed_standard(frame); + break; + default: + break; } } @@ -312,22 +315,24 @@ void GnssReceiver::parse_speed_standard(const isobus::CANMessageFrame &frame) void GnssReceiver::parse_heading_FE45(const isobus::CANMessageFrame &frame) { - if (frame.dataLength < 2) + if (frame.dataLength < 3) return; - std::uint16_t raw_heading = static_cast( + std::uint16_t raw_coarse = static_cast( static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8)); - if (raw_heading == 0xFFFF) + if (raw_coarse == 0xFFFF) return; - // Deere proprietary scale: raw / 27.5 degrees (confirmed via RE capture) - double heading = raw_heading / 27.5; + double coarse = raw_coarse * 0.02807; + double fraction = frame.data[2] / 256.0; + double heading = coarse + fraction; std::lock_guard lock(dataMutex); if (!data.has_heading) { - std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading << " deg (raw=" << raw_heading << ")" << std::endl; + std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading + << " deg (coarse_raw=" << raw_coarse << " frac_byte=" << static_cast(frame.data[2]) << ")" << std::endl; } data.heading_deg = heading; data.has_heading = true; @@ -586,21 +591,19 @@ void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) return; } - std::string sentence = build_panda(snapshot); + std::string sentence = build_gga(snapshot); if (!pandaSending) { - std::cout << "[GnssReceiver] Sending first $PANDA sentence: " << sentence.substr(0, sentence.size() - 2) << std::endl; + std::cout << "[GnssReceiver] Sending first $GPGGA sentence: " << sentence.substr(0, sentence.size() - 2) << std::endl; pandaSending = true; } if (isobus::SystemTiming::time_expired_ms(lastPandaLog, 10000)) { - std::cout << "[GnssReceiver] $PANDA: lat=" << snapshot.latitude_deg + std::cout << "[GnssReceiver] GGA: lat=" << snapshot.latitude_deg << " lon=" << snapshot.longitude_deg << " alt=" << snapshot.altitude_m - << " hdg=" << snapshot.heading_deg - << " spd=" << snapshot.speed_kmh << "km/h" << std::endl; lastPandaLog = isobus::SystemTiming::get_timestamp_ms(); } @@ -609,7 +612,7 @@ void GnssReceiver::send_panda_if_ready(std::shared_ptr udp) lastPandaSend = isobus::SystemTiming::get_timestamp_ms(); } -std::string GnssReceiver::build_panda(const GnssData &snapshot) const +std::string GnssReceiver::build_gga(const GnssData &snapshot) const { // --- Time: HHMMSS.CC --- std::uint8_t ss = (snapshot.minute_ms / 1000) % 60; @@ -636,35 +639,19 @@ std::string GnssReceiver::build_panda(const GnssData &snapshot) const // --- Altitude --- char altStr[16]; - std::snprintf(altStr, sizeof(altStr), "%.2f", snapshot.has_altitude ? snapshot.altitude_m : 0.0); - - // --- Speed (km/h -> knots) --- - double speedKnots = snapshot.has_speed ? snapshot.speed_kmh / 1.852 : 0.0; - char speedStr[16]; - std::snprintf(speedStr, sizeof(speedStr), "%.2f", speedKnots); - - // --- Heading --- - char headingStr[16]; - std::snprintf(headingStr, sizeof(headingStr), "%.1f", snapshot.has_heading ? snapshot.heading_deg : 0.0); + std::snprintf(altStr, sizeof(altStr), "%.1f", snapshot.has_altitude ? snapshot.altitude_m : 0.0); - // --- Assemble body (everything between $ and *) --- + // --- Assemble body: GPGGA,time,lat,N,lon,E,fix,sats,hdop,alt,M,geoidSep,M,ageDGPS,refStation --- char body[256]; std::snprintf(body, sizeof(body), - "PANDA,%s,%s,%c,%s,%c,%d,%d,%.1f,%s,%.1f,%s,%s,%.1f,%.1f,%.1f", + "GPGGA,%s,%s,%c,%s,%c,%d,%02d,%.1f,%s,M,0.0,M,,", timeStr, latStr, latHemi, lonStr, lonHemi, - 4, // fixType (RTK default) - 18, // satellites default - 0.8, // HDOP default - altStr, - 0.0, // ageDGPS - speedStr, - headingStr, - 0.0, // roll - 0.0, // pitch - 0.0 // yawRate - ); + 4, // fix quality (4=RTK) + 18, // satellites + 0.8, // HDOP + altStr); // --- Checksum: XOR of all chars in body --- std::uint8_t cs = 0; @@ -676,7 +663,6 @@ std::string GnssReceiver::build_panda(const GnssData &snapshot) const char checksum[4]; std::snprintf(checksum, sizeof(checksum), "%02X", cs); - // --- Final sentence --- std::string sentence = "$"; sentence += body; sentence += "*"; From 0832f5e52ce0341fcfb3d81815b60f505545bb70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 22:57:26 +0100 Subject: [PATCH 8/9] heading now really? --- src/gnss_receiver.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index d0f0789..18ee9b1 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -185,6 +185,8 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) // Standard J1939 GNSS PGNs - accept from ANY source address // (SF3000 at SA=0x9A uses proprietary PGNs, but other ECUs on the bus may broadcast these) + // NOTE: PGN 0xFEF2 (65266) is "Fuel Economy" in J1939, NOT altitude. + // Altitude source from SF3000 proprietary PGNs is TBD. switch (pgn) { case 0xFEF3: @@ -193,9 +195,6 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) case 0xFEF0: parse_time_date_standard(frame); break; - case 0xFEF2: - parse_altitude_standard(frame); - break; case 0xFEF1: parse_speed_standard(frame); break; From 3dad3b32719785cacc4f01b7f6d0ef8c24e7da61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gunics=20Bal=C3=A1zs?= Date: Sat, 14 Mar 2026 23:14:10 +0100 Subject: [PATCH 9/9] please be the right heading --- include/gnss_receiver.hpp | 3 +- src/gnss_receiver.cpp | 126 ++++++++++++++++++++++++++++---------- 2 files changed, 97 insertions(+), 32 deletions(-) diff --git a/include/gnss_receiver.hpp b/include/gnss_receiver.hpp index b0348ee..788beab 100644 --- a/include/gnss_receiver.hpp +++ b/include/gnss_receiver.hpp @@ -73,7 +73,8 @@ class GnssReceiver void parse_speed_standard(const isobus::CANMessageFrame &frame); // Confirmed/candidate parsers for proprietary Deere PGNs - void parse_heading_FE45(const isobus::CANMessageFrame &frame); + void parse_heading_FE48(const isobus::CANMessageFrame &frame); + void parse_candidate_FE45(const isobus::CANMessageFrame &frame); void parse_candidate_FE43(const isobus::CANMessageFrame &frame); void parse_candidate_FE12(const isobus::CANMessageFrame &frame); void parse_candidate_FE13(const isobus::CANMessageFrame &frame); diff --git a/src/gnss_receiver.cpp b/src/gnss_receiver.cpp index 18ee9b1..6fff739 100644 --- a/src/gnss_receiver.cpp +++ b/src/gnss_receiver.cpp @@ -140,12 +140,15 @@ void GnssReceiver::on_can_frame(const isobus::CANMessageFrame &frame) // Dispatch to parsers for SF3000 proprietary PGNs switch (pgn) { - // Confirmed: heading - case 0xFE45: - parse_heading_FE45(frame); + // Confirmed: heading (scale 0.336 derived from drive log) + case 0xFE48: + parse_heading_FE48(frame); break; // Proprietary Deere PGNs - candidate parsers + case 0xFE45: + parse_candidate_FE45(frame); + break; case 0xFE43: parse_candidate_FE43(frame); break; @@ -309,10 +312,40 @@ void GnssReceiver::parse_speed_standard(const isobus::CANMessageFrame &frame) } // ============================================================================ -// Confirmed/candidate: PGN 0xFE45 - Heading +// Confirmed: PGN 0xFE48 - Heading (scale 0.336, derived from drive log) // ============================================================================ -void GnssReceiver::parse_heading_FE45(const isobus::CANMessageFrame &frame) +void GnssReceiver::parse_heading_FE48(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 2) + return; + + std::uint16_t raw = static_cast( + frame.data[0] | (frame.data[1] << 8)); + + if (raw == 0xFFFF) + return; + + double heading = raw * 0.336; + + if (heading >= 360.0) + heading -= 360.0; + + std::lock_guard lock(dataMutex); + if (!data.has_heading) + { + std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE48 *** " + << heading << " deg (raw=" << raw << ")" << std::endl; + } + data.heading_deg = heading; + data.has_heading = true; +} + +// ============================================================================ +// Demoted candidate: PGN 0xFE45 - old heading (kept for RE logging) +// ============================================================================ + +void GnssReceiver::parse_candidate_FE45(const isobus::CANMessageFrame &frame) { if (frame.dataLength < 3) return; @@ -327,14 +360,13 @@ void GnssReceiver::parse_heading_FE45(const isobus::CANMessageFrame &frame) double fraction = frame.data[2] / 256.0; double heading = coarse + fraction; - std::lock_guard lock(dataMutex); - if (!data.has_heading) + static bool logged = false; + if (!logged) { - std::cout << "[GnssReceiver] *** HEADING set by PGN 0xFE45 *** " << heading - << " deg (coarse_raw=" << raw_coarse << " frac_byte=" << static_cast(frame.data[2]) << ")" << std::endl; + std::cout << "[GnssReceiver] PGN 0xFE45 candidate (demoted): heading=" << heading + << " deg (coarse_raw=" << raw_coarse << " frac=" << static_cast(frame.data[2]) << ")" << std::endl; + logged = true; } - data.heading_deg = heading; - data.has_heading = true; } // ============================================================================ @@ -447,31 +479,63 @@ void GnssReceiver::parse_candidate_F022(const isobus::CANMessageFrame &frame) void GnssReceiver::parse_candidate_FFFA(const isobus::CANMessageFrame &frame) { - // PGN 0xFFFA (65530) - Deere proprietary, possibly GNSS position or extended data + // PGN 0xFFFA (65530) - Deere proprietary, ~22 Hz, heading candidate if (frame.dataLength < 8) return; - // Try interpreting as two uint32 position fields (same encoding as PGN 0xFEF3) - std::uint32_t dw0 = - static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8) | - (static_cast(frame.data[2]) << 16) | (static_cast(frame.data[3]) << 24); - std::uint32_t dw1 = - static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8) | - (static_cast(frame.data[6]) << 16) | (static_cast(frame.data[7]) << 24); + // Extract all word-sized fields for heading analysis + std::uint16_t w0 = static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8); + std::uint16_t w1 = static_cast(frame.data[2]) | (static_cast(frame.data[3]) << 8); + std::uint16_t w2 = static_cast(frame.data[4]) | (static_cast(frame.data[5]) << 8); + std::uint16_t w3 = static_cast(frame.data[6]) | (static_cast(frame.data[7]) << 8); + + // Try many heading scales on each word pair — target ≈ 278° + // FE45-style: raw * 0.02807 + double w0_fe45 = w0 * 0.02807; + double w1_fe45 = w1 * 0.02807; + double w2_fe45 = w2 * 0.02807; + double w3_fe45 = w3 * 0.02807; + + // J1939 SPN 584 (Vehicle Heading): raw * (1/128) deg + double w0_128 = w0 / 128.0; + double w1_128 = w1 / 128.0; + double w2_128 = w2 / 128.0; + double w3_128 = w3 / 128.0; + + // 0.01 deg scale + double w0_001 = w0 * 0.01; + double w1_001 = w1 * 0.01; + double w2_001 = w2 * 0.01; + double w3_001 = w3 * 0.01; + + // Log periodically (every 5s) for live RE monitoring + static std::uint32_t lastFFFALog = 0; + static bool firstLog = true; + if (firstLog || isobus::SystemTiming::time_expired_ms(lastFFFALog, 5000)) + { + std::cout << "[GnssReceiver] PGN 0xFFFA RE dump: [" + << std::hex << std::setw(2) << std::setfill('0') << std::uppercase; + for (int i = 0; i < 8; i++) + { + if (i > 0) std::cout << " "; + std::cout << std::setw(2) << static_cast(frame.data[i]); + } + std::cout << "]" << std::dec << std::endl; - double as_lat = dw0 * 1e-7 - 210.0; - double as_lon = dw1 * 1e-7 - 210.0; + std::cout << " w0=0x" << std::hex << w0 << " w1=0x" << w1 + << " w2=0x" << w2 << " w3=0x" << w3 << std::dec << std::endl; - static bool logged = false; - if (!logged) - { - std::cout << "[GnssReceiver] PGN 0xFFFA candidate: dw0=0x" << std::hex << dw0 - << " dw1=0x" << dw1 << std::dec - << " as_lat=" << as_lat - << " as_lon=" << as_lon - << " dw0_as_alt_cm=" << (dw0 * 0.01) - << std::endl; - logged = true; + std::cout << " FE45-scale(*0.02807): w0=" << std::fixed << std::setprecision(2) + << w0_fe45 << " w1=" << w1_fe45 << " w2=" << w2_fe45 << " w3=" << w3_fe45 << std::endl; + + std::cout << " 1/128-scale: w0=" << w0_128 << " w1=" << w1_128 + << " w2=" << w2_128 << " w3=" << w3_128 << std::endl; + + std::cout << " 0.01-scale: w0=" << w0_001 << " w1=" << w1_001 + << " w2=" << w2_001 << " w3=" << w3_001 << std::endl; + + lastFFFALog = isobus::SystemTiming::get_timestamp_ms(); + firstLog = false; } }