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..788beab --- /dev/null +++ b/include/gnss_receiver.hpp @@ -0,0 +1,108 @@ +/** + * @author Qoder + * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences + * @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" +#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(). + /// @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 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); + +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; + + bool has_position = false; + bool has_time = false; + bool has_altitude = false; + bool has_heading = false; + 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); + + // 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_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); + 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_gga(const GnssData &snapshot) const; + + static constexpr std::uint8_t SF3000_SOURCE_ADDRESS = 0x9A; + + GnssData data; + std::mutex dataMutex; + isobus::EventCallbackHandle canFrameHandle = 0; + 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/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..7d45112 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(true); // RE mode: log all SF3000 frames to CSV + 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..6fff739 --- /dev/null +++ b/src/gnss_receiver.cpp @@ -0,0 +1,736 @@ +/** + * @author Qoder + * @brief GNSS receiver that parses J1939 PGNs from a John Deere SF3000 and generates $PANDA NMEA sentences + * @version 0.2 + * @date 2025-03-14 + */ + +#include "gnss_receiver.hpp" + +#include +#include +#include +#include +#include + +#include "isobus/utility/system_timing.hpp" + +GnssReceiver::GnssReceiver() +{ +} + +GnssReceiver::~GnssReceiver() +{ + isobus::CANHardwareInterface::get_can_frame_received_event_dispatcher() + .remove_listener(canFrameHandle); + if (csvFile.is_open()) + { + csvFile.close(); + } +} + +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); + }); + std::cout << "[GnssReceiver] CAN frame listener registered for SF3000 (SA=0x" + << 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) + return; + + std::uint8_t sa = frame.identifier & 0xFF; + std::uint8_t pf = (frame.identifier >> 16) & 0xFF; + + std::uint32_t pgn; + if (pf >= 0xF0) + { + pgn = (frame.identifier >> 8) & 0x3FFFF; + } + else + { + pgn = (frame.identifier >> 8) & 0x3FF00; + } + + // 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 SF3000 proprietary PGNs + switch (pgn) + { + // 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; + 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; + } + } + + // 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); + } + + // 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: + parse_position_standard(frame); + break; + case 0xFEF0: + parse_time_date_standard(frame); + break; + case 0xFEF1: + parse_speed_standard(frame); + break; + default: + break; + } +} + +// ============================================================================ +// 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; + + 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); + 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); + + if (raw_lat == 0xFFFFFFFF || raw_lon == 0xFFFFFFFF) + return; + + 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) + { + 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] *** 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_standard(const isobus::CANMessageFrame &frame) +{ + if (frame.dataLength < 4) + return; + + 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]; + + if (hr > 23 || min > 59 || ms_of_minute > 59999) + return; + + std::lock_guard lock(dataMutex); + if (!data.has_time) + { + std::cout << "[GnssReceiver] *** TIME set by PGN 0xFEF0 *** " + << static_cast(hr) << ":" << static_cast(min) << " (" << ms_of_minute << "ms)" << std::endl; + } + data.hour = hr; + data.minute = min; + data.minute_ms = ms_of_minute; + data.has_time = true; +} + +void GnssReceiver::parse_altitude_standard(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); + if (!data.has_altitude) + { + 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_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: PGN 0xFE48 - Heading (scale 0.336, derived from drive log) +// ============================================================================ + +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; + + std::uint16_t raw_coarse = static_cast( + static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8)); + + if (raw_coarse == 0xFFFF) + return; + + double coarse = raw_coarse * 0.02807; + double fraction = frame.data[2] / 256.0; + double heading = coarse + fraction; + + static bool logged = false; + if (!logged) + { + std::cout << "[GnssReceiver] PGN 0xFE45 candidate (demoted): heading=" << heading + << " deg (coarse_raw=" << raw_coarse << " frac=" << static_cast(frame.data[2]) << ")" << std::endl; + logged = true; + } +} + +// ============================================================================ +// Proprietary Deere candidate parsers +// Each logs decoded candidate fields. Actual field mapping TBD via RE. +// ============================================================================ + +void GnssReceiver::parse_candidate_FE43(const isobus::CANMessageFrame &frame) +{ + // PGN 0xFE43 (65091) - Deere proprietary, possibly vehicle dynamics or tilt + // Expected: near Kecskemet, tilt = -1.2 deg + if (frame.dataLength < 2) + return; + + 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? + + 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::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] PGN 0xFE12 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_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, ~22 Hz, heading candidate + if (frame.dataLength < 8) + return; + + // 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; + + std::cout << " w0=0x" << std::hex << w0 << " w1=0x" << w1 + << " w2=0x" << w2 << " w3=0x" << w3 << std::dec << std::endl; + + 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; + } +} + +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); + snapshot = data; + } + + if (!snapshot.has_position) + { + if (isobus::SystemTiming::time_expired_ms(lastWaitingLog, 5000)) + { + 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; + } + + std::string sentence = build_gga(snapshot); + + if (!pandaSending) + { + 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] GGA: lat=" << snapshot.latitude_deg + << " lon=" << snapshot.longitude_deg + << " alt=" << snapshot.altitude_m + << std::endl; + lastPandaLog = isobus::SystemTiming::get_timestamp_ms(); + } + + udp->send_raw(sentence); + lastPandaSend = isobus::SystemTiming::get_timestamp_ms(); +} + +std::string GnssReceiver::build_gga(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), "%.1f", snapshot.has_altitude ? snapshot.altitude_m : 0.0); + + // --- 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), + "GPGGA,%s,%s,%c,%s,%c,%d,%02d,%.1f,%s,M,0.0,M,,", + timeStr, + latStr, latHemi, + lonStr, lonHemi, + 4, // fix quality (4=RTK) + 18, // satellites + 0.8, // HDOP + altStr); + + // --- 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); + + 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; +}