From 9d21c0b6c1cc3b35038180e2a61d0dd501cb8bb7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 19 Nov 2021 07:49:21 +1100
Subject: [PATCH] AP_GPS: added GPS data logging system

for debugging GPS protocols
---
 libraries/AP_GPS/AP_GPS_NMEA.cpp  | 15 +++---------
 libraries/AP_GPS/AP_GPS_UBLOX.cpp |  3 +++
 libraries/AP_GPS/GPS_Backend.cpp  | 38 +++++++++++++++++++++++++++++++
 libraries/AP_GPS/GPS_Backend.h    | 24 +++++++++++++++++++
 4 files changed, 68 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 3d7919ce89..4fe39b1b30 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -40,9 +40,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-// optionally log all NMEA data for debug purposes
-// #define NMEA_LOG_PATH "nmea.log"
-
 // Convenience macros //////////////////////////////////////////////////////////
 //
 #define DIGIT_TO_VAL(_x)        (_x - '0')
@@ -56,18 +53,12 @@ bool AP_GPS_NMEA::read(void)
     numc = port->available();
     while (numc--) {
         char c = port->read();
-#ifdef NMEA_LOG_PATH
-        static FILE *logf = nullptr;
-        if (logf == nullptr) {
-            logf = fopen(NMEA_LOG_PATH, "wb");
-        }
-        if (logf != nullptr) {
-            ::fwrite(&c, 1, 1, logf);
-        }
-#endif
         if (_decode(c)) {
             parsed = true;
         }
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+        log_data((const uint8_t *)&c, 1);
+#endif
     }
     return parsed;
 }
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 7fd64ad1e6..e960391743 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -587,6 +587,9 @@ AP_GPS_UBLOX::read(void)
             break;
         }
         const uint8_t data = rdata;
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+        log_data(&data, 1);
+#endif
 
 #if GPS_MOVING_BASELINE
         if (rtcm3_parser) {
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index 968e0ee4f9..826b5304f1 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/libraries/AP_GPS/GPS_Backend.cpp
@@ -30,6 +30,10 @@
 
 #include <GCS_MAVLink/GCS.h>
 
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+#include <AP_Filesystem/AP_Filesystem.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
@@ -398,3 +402,37 @@ bad_yaw:
     return false;
 }
 #endif // GPS_MOVING_BASELINE
+
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+
+// log some data for debugging
+void AP_GPS_Backend::log_data(const uint8_t *data, uint16_t length)
+{
+    logging.buf.write(data, length);
+    if (!logging.io_registered) {
+        logging.io_registered = true;
+        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_update, void));
+    }
+}
+
+// IO thread update, writing to log file
+void AP_GPS_Backend::logging_update(void)
+{
+    if (logging.fd == -1) {
+        char fname[] = "gpsN.log";
+        fname[3] = '1' + state.instance;
+        logging.fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND);
+    }
+    if (logging.fd != -1) {
+        uint32_t n = 0;
+        const uint8_t *p = logging.buf.readptr(n);
+        if (p != nullptr && n != 0) {
+            int32_t written = AP::FS().write(logging.fd, p, n);
+            if (written > 0) {
+                logging.buf.advance(written);
+                AP::FS().fsync(logging.fd);
+            }
+        }
+    }
+}
+#endif // AP_GPS_DEBUG_LOGGING_ENABLED
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index 6510482ad8..7ebea322f1 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -22,6 +22,16 @@
 #include <AP_RTC/JitterCorrection.h>
 #include "AP_GPS.h"
 
+#ifndef AP_GPS_DEBUG_LOGGING_ENABLED
+// enable this to log all bytes from the GPS. Also needs a call to
+// log_data() in each backend
+#define AP_GPS_DEBUG_LOGGING_ENABLED 0
+#endif
+
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+#include <AP_HAL/utility/RingBuffer.h>
+#endif
+
 class AP_GPS_Backend
 {
 public:
@@ -140,6 +150,11 @@ protected:
         return gps.get_type(state.instance);
     }
 
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+    // log some data for debugging
+    void log_data(const uint8_t *data, uint16_t length);
+#endif
+
 private:
     // itow from previous message
     uint32_t _last_itow;
@@ -150,4 +165,13 @@ private:
     uint16_t _rate_counter;
 
     JitterCorrection jitter_correction;
+
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+    struct {
+        int fd = -1;
+        ByteBuffer buf{32768};
+        bool io_registered;
+    } logging;
+    void logging_update(void);
+#endif
 };