GPS - Refactor update() to remove funcs that should be called by the driver, make it less reliant on iface type and driver type, handle that all in hardware

Author: brentru

src/components/gps/controller.cpp

@@ -106,54 +106,6 @@ bool GPSController::AddGPS(HardwareSerial *serial,
   return true;
 }
 
-/*!
- * @brief Pushes a new NMEA sentence into the circular buffer.
- * @param new_sentence Pointer to the new NMEA sentence to be added.
- * @return 0 on success, -1 if the buffer is full.
- */
-int GPSController::NmeaBufPush(const char *new_sentence) {
-  if (!new_sentence)
-    return -1;
-
-  int next = _nmea_buff.head + 1; // points to head after the current write
-  if (next >= _nmea_buff.maxlen)
-    next = 0; // wrap around
-
-  // If buffer is full, advance tail to overwrite oldest data
-  if (next == _nmea_buff.tail) {
-    _nmea_buff.tail = (_nmea_buff.tail + 1) % _nmea_buff.maxlen;
-  }
-
-  // Copy the new sentence into the buffer
-  strncpy(_nmea_buff.sentences[_nmea_buff.head], new_sentence,
-          MAX_LEN_NMEA_SENTENCE - 1);
-  _nmea_buff.sentences[_nmea_buff.head][MAX_LEN_NMEA_SENTENCE - 1] = '\0';
-  _nmea_buff.head = next;
-  return 0;
-}
-
-/*!
- * @brief Pops a NMEA sentence from the circular buffer, FIFO order.
- * @param sentence Pointer to the buffer where the popped sentence will be
- * stored.
- * @return 0 on success, -1 if the buffer is empty.
- */
-int GPSController::NmeaBufPop(char *sentence) {
-  // Is the buffer empty?
-  if (_nmea_buff.head == _nmea_buff.tail)
-    return -1;
-
-  int next =
-      _nmea_buff.tail + 1; // next is where tail will point to after this read.
-  if (next >= _nmea_buff.maxlen)
-    next = 0;
-
-  // Copy sentence from tail
-  strcpy(sentence, _nmea_buff.sentences[_nmea_buff.tail]);
-  _nmea_buff.tail = next; // Advance tail
-  return 0;
-}
-
 /*!
  * @brief Updates the GPSController, polling the GPS hardware for data.
  * This function checks if the read period has elapsed and processes the GPS
@@ -203,41 +155,17 @@ void GPSController::update() {
       continue; // Not yet elapsed, skip this driver
 
     // Discard the GPS buffer before we attempt to do a fresh read
-    if (drv->GetIfaceType() == GPS_IFACE_UART_HW) {
-      // TODO: Refactor this into a function within hardware.cpp
-      size_t bytes_avail = ada_gps->available();
-      if (bytes_avail > 0) {
-        for (size_t i = 0; i < bytes_avail; i++) {
-          ada_gps->read();
-        }
-      }
-    } else if (drv->GetIfaceType() == GPS_IFACE_I2C) {
-      drv->I2cReadDiscard();
-    }
-
-    // Unset the RX flag
-    if (ada_gps->newNMEAreceived()) {
-      ada_gps->lastNMEA();
-    }
+    drv->ReadDiscardBuffer();
 
-    // Read from the GPS module for update_rate milliseconds
-    ulong update_rate = 1000 / drv->GetNmeaUpdateRate();
-    ulong start_time = millis();
-    while (millis() - start_time < update_rate) {
-      char c = ada_gps->read();
-      // Check if we have a new NMEA sentence
-      if (ada_gps->newNMEAreceived()) {
-        // If we have a new sentence, push it to the buffer
-        char *last_nmea = ada_gps->lastNMEA();
-        NmeaBufPush(ada_gps->lastNMEA());
-      }
-    }
+    // Poll the GPS hardware for update_rate ms
+    // and store NMEA sentences in a ring buffer
+    drv->PollStoreSentences();
 
     // Parse each NMEA sentence in the buffer
     _gps_model->CreateGPSEvent();
     char nmea_sentence[MAX_LEN_NMEA_SENTENCE];
     bool has_gps_event = false;
-    while (NmeaBufPop(nmea_sentence) != -1) {
+    while (drv->NmeaBufPop(nmea_sentence) != -1) {
       // Parse the NMEA sentence
       WS_DEBUG_PRINT("[gps] Parsing NMEA sentence: ");
       WS_DEBUG_PRINTLN(nmea_sentence);

src/components/gps/controller.h

@@ -20,16 +20,6 @@
 #include "model.h"
 #include <Adafruit_GPS.h>
 
-#define MAX_NMEA_SENTENCES 10    ///< Size of the NMEA buffer
-#define MAX_LEN_NMEA_SENTENCE 82 ///< Maximum length of a NMEA sentence
-
-typedef struct {
-  char sentences[MAX_NMEA_SENTENCES][MAX_LEN_NMEA_SENTENCE];
-  int head;
-  int tail;
-  int maxlen;
-} nmea_buffer_t;
-
 class Wippersnapper_V2; ///< Forward declaration
 class GPSModel;         ///< Forward declaration
 class GPSHardware;      ///< Forward declaration
@@ -47,11 +37,8 @@ class GPSController {
   void update();
 
 private:
-  int NmeaBufPush(const char *new_sentence);
-  int NmeaBufPop(char *sentence);
   GPSModel *_gps_model;                    ///< GPS model instance
   std::vector<GPSHardware *> _gps_drivers; ///< GPS hardware instances
-  nmea_buffer_t _nmea_buff; ///< NMEA buffer for storing sentences
 };
 extern Wippersnapper_V2 WsV2; ///< Wippersnapper V2 instance
 #endif                        // WS_GPS_CONTROLLER_H

src/components/gps/hardware.cpp

@@ -505,4 +505,109 @@ GpsDriverType GPSHardware::GetDriverType() { return _driver_type; }
  * @brief Returns the interface type of the GPS hardware.
  * @returns The interface type of the GPS hardware.
  */
-GpsInterfaceType GPSHardware::GetIfaceType() { return _iface_type; }
+GpsInterfaceType GPSHardware::GetIfaceType() { return _iface_type; }
+
+/*!
+ * @brief Discards any data in the UART RX buffer.
+ */
+void GPSHardware::UartReadDiscard() {
+  if (_driver_type == GPS_DRV_MTK) {
+    size_t bytes_avail = _ada_gps->available();
+    if (bytes_avail > 0) {
+      for (size_t i = 0; i < bytes_avail; i++) {
+        _ada_gps->read();
+      }
+    }
+  }
+  // TODO: Support UBX's UART iface here
+}
+
+/*!
+ * @brief Discards any data in the UART or I2C RX buffer.
+ */
+void GPSHardware::ReadDiscardBuffer() {
+  if (_iface_type == GPS_IFACE_UART_HW) {
+    UartReadDiscard();
+  } else if (_iface_type == GPS_IFACE_I2C) {
+    I2cReadDiscard();
+  } else {
+    WS_DEBUG_PRINTLN("[gps] ERROR: Unsupported GPS interface type!");
+  }
+}
+
+/*!
+ * @brief Polls the GPS hardware for new NMEA sentences and stores them in a
+ * circular buffer.
+ */
+void GPSHardware::PollStoreSentences() {
+  if (_driver_type == GPS_DRV_MTK) {
+    // Before we poll, Unset the RX flag
+    if (_ada_gps->newNMEAreceived())
+      _ada_gps->lastNMEA();
+
+    // Read from the GPS module for update_rate milliseconds
+    ulong update_rate = 1000 / _nmea_update_rate;
+    ulong start_time = millis();
+    while (millis() - start_time < update_rate) {
+      char c = _ada_gps->read();
+      // Check if we have a new NMEA sentence
+      if (_ada_gps->newNMEAreceived()) {
+        // If we have a new sentence, push it to the buffer
+        char *last_nmea = _ada_gps->lastNMEA();
+        NmeaBufPush(_ada_gps->lastNMEA());
+      }
+    }
+  } else if (_driver_type == GPS_DRV_UBLOX) {
+    // TODO!
+  } else {
+    WS_DEBUG_PRINTLN("[gps] ERROR: Unsupported GPS driver type for polling!");
+  }
+}
+
+/*!
+ * @brief Pushes a new NMEA sentence into the circular buffer.
+ * @param new_sentence Pointer to the new NMEA sentence to be added.
+ * @return 0 on success, -1 if the buffer is full.
+ */
+int GPSHardware::NmeaBufPush(const char *new_sentence) {
+  if (!new_sentence)
+    return -1;
+
+  int next = _nmea_buff.head + 1; // points to head after the current write
+  if (next >= _nmea_buff.maxlen)
+    next = 0; // wrap around
+
+  // If buffer is full, advance tail to overwrite oldest data
+  if (next == _nmea_buff.tail) {
+    _nmea_buff.tail = (_nmea_buff.tail + 1) % _nmea_buff.maxlen;
+  }
+
+  // Copy the new sentence into the buffer
+  strncpy(_nmea_buff.sentences[_nmea_buff.head], new_sentence,
+          MAX_LEN_NMEA_SENTENCE - 1);
+  _nmea_buff.sentences[_nmea_buff.head][MAX_LEN_NMEA_SENTENCE - 1] = '\0';
+  _nmea_buff.head = next;
+  return 0;
+}
+
+/*!
+ * @brief Pops a NMEA sentence from the circular buffer, FIFO order.
+ * @param sentence Pointer to the buffer where the popped sentence will be
+ * stored.
+ * @return 0 on success, -1 if the buffer is empty.
+ */
+int GPSHardware::NmeaBufPop(char *sentence) {
+  // Is the buffer empty?
+  if (_nmea_buff.head == _nmea_buff.tail)
+    return -1;
+
+  int next =
+      _nmea_buff.tail + 1; // next is where tail will point to after this read.
+  if (next >= _nmea_buff.maxlen)
+    next = 0;
+
+  // Copy sentence from tail
+  strcpy(sentence, _nmea_buff.sentences[_nmea_buff.tail]);
+  _nmea_buff.tail = next; // Advance tail
+  return 0;
+}

src/components/gps/hardware.h

@@ -29,7 +29,9 @@
 #define DEFAULT_MTK_NMEA_BAUD_RATE 9600 ///< Default NMEA baud rate in bits per
 #define MAX_NEMA_SENTENCE_LEN 82        ///< Maximum length of a NMEA sentence
 #define PA1010D_I2C_ADDRESS 0x10        ///< I2C address for PA1010D GPS module
-#define UBX_I2C_ADDRESS 0x42 ///< I2C address for all u-Blox GPS products
+#define UBX_I2C_ADDRESS 0x42     ///< I2C address for all u-Blox GPS products
+#define MAX_NMEA_SENTENCES 10    ///< Size of the NMEA buffer
+#define MAX_LEN_NMEA_SENTENCE 82 ///< Maximum length of a NMEA sentence
 
 class Wippersnapper_V2; ///< Forward declaration
 class UARTHardware;     ///< Forward declaration
@@ -48,6 +50,13 @@ enum GpsDriverType {
   GPS_DRV_GENERIC_NMEA ///< Generic NMEA GPS driver
 }; ///< Type of GPS driver used
 
+typedef struct {
+  char sentences[MAX_NMEA_SENTENCES][MAX_LEN_NMEA_SENTENCE];
+  int head;
+  int tail;
+  int maxlen;
+} nmea_buffer_t; ///< NMEA sentence ring buffer structure
+
 /*!
     @brief  Low-level hardware interface for WipperSnapper's generic GPS
             component. This class handles the communication with the GPS module
@@ -77,14 +86,20 @@ class GPSHardware {
   SFE_UBLOX_GNSS *GetUbxGps();
   GpsDriverType GetDriverType();
   GpsInterfaceType GetIfaceType();
-  void I2cReadDiscard();
+  // HAL Abstraction for GPS driver commands
+  // Used to abstract the Adafruit_GPS and SFE_UBLOX_GNSS libraries
+  // and intelligently handle the differences between them
+  void ReadDiscardBuffer();
+  void PollStoreSentences();
 
 private:
   bool QueryModuleType();
   bool DetectMtkUart();
   bool DetectMtkI2C(uint32_t addr);
   bool DetectUbxI2C(uint32_t addr);
   bool BuildPmtkAck(char *msg_cmd, char *msg_resp);
+  void I2cReadDiscard();
+  void UartReadDiscard();
   GpsInterfaceType _iface_type;         ///< Type of interface used by GPS
   GpsDriverType _driver_type;           ///< Type of GPS driver used by GPS
   HardwareSerial *_hw_serial = nullptr; ///< Optional HardwareSerial instance
@@ -100,6 +115,11 @@ class GPSHardware {
   MicroNMEA
       *_micro_nmea; ///< Optional MicroNMEA instance for parsing NMEA sentences
   char _micro_nmea_buf[100]; ///< Optional Buffer for MicroNMEA parsing
+  // NMEA sentence ring buffer
+  int NmeaBufPush(
+      const char *new_sentence);  ///< Push a sentence to the NMEA buffer
+  int NmeaBufPop(char *sentence); ///< Pop a sentence from the NMEA buffer
+  nmea_buffer_t _nmea_buff;       ///< NMEA buffer for storing sentences
 };
 extern Wippersnapper_V2 WsV2; ///< Wippersnapper V2 instance
 #endif                        // WS_GPS_HARDWARE_H