From 4ffb40a972052e3151bbc33d478c5fcb3de66c19 Mon Sep 17 00:00:00 2001
From: Clemens Elflein <clemens1@familie-elflein.de>
Date: Mon, 11 Nov 2024 00:46:34 +0100
Subject: [PATCH] added TCP interface for GPS

---
 src/drivers/gps/gps_driver.cpp     | 14 ++++++++++++--
 src/drivers/gps/gps_driver.h       |  7 +++++--
 src/drivers/gps/ublox_gps_driver.h |  4 ++--
 3 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/src/drivers/gps/gps_driver.cpp b/src/drivers/gps/gps_driver.cpp
index 2a8230c..88c013e 100644
--- a/src/drivers/gps/gps_driver.cpp
+++ b/src/drivers/gps/gps_driver.cpp
@@ -10,6 +10,11 @@
 
 namespace xbot::driver::gps {
 
+void GpsDriver::RawDataInput(uint8_t *data, size_t size) {
+  if(!IsRawMode())
+    return;
+  send_raw(data, size);
+}
 GpsDriver::GpsDriver() {
   datum_lat_ = datum_long_ = NAN;
 }
@@ -74,8 +79,9 @@ void GpsDriver::SetDatum(double datum_lat, double datum_long, double datum_heigh
 }
 
 bool GpsDriver::send_raw(const void *data, size_t size) {
-  // sdWrite(uart, static_cast<const uint8_t*>(data), size);
+  chMtxLock(&mutex_);
   uartSendFullTimeout(uart_, &size, data, TIME_INFINITE);
+  chMtxUnlock(&mutex_);
   return true;
 }
 
@@ -118,8 +124,12 @@ void GpsDriver::threadFunc() {
     }
     if (processing_buffer_len_ > 0) {
       ProcessBytes(processing_buffer_, processing_buffer_len_);
-      last_ndtr = 0;
+      if(IsRawMode()) {
+        RawDataOutput(processing_buffer_, processing_buffer_len_);
+      }
     }
+    last_ndtr = 0;
+    processing_buffer_len_ = 0;
     processing_done_ = true;
   }
 }
diff --git a/src/drivers/gps/gps_driver.h b/src/drivers/gps/gps_driver.h
index 56c8c56..2a0acfe 100644
--- a/src/drivers/gps/gps_driver.h
+++ b/src/drivers/gps/gps_driver.h
@@ -7,15 +7,18 @@
 
 #include <etl/delegate.h>
 
+#include <debug/debuggable_driver.hpp>
+
 #include "ch.h"
 #include "hal.h"
 
 namespace xbot::driver::gps {
-class GpsDriver {
+class GpsDriver : public DebuggableDriver {
  public:
+  void RawDataInput(uint8_t *data, size_t size) override;
   explicit GpsDriver();
 
-  virtual ~GpsDriver() = default;
+  ~GpsDriver() override = default;
 
   /*
    * The final GPS state we're interested in.
diff --git a/src/drivers/gps/ublox_gps_driver.h b/src/drivers/gps/ublox_gps_driver.h
index 3756155..65af11e 100644
--- a/src/drivers/gps/ublox_gps_driver.h
+++ b/src/drivers/gps/ublox_gps_driver.h
@@ -6,11 +6,11 @@
 #ifndef XBOT_DRIVER_GPS_UBLOX_GPS_DRIVER_H
 #define XBOT_DRIVER_GPS_UBLOX_GPS_DRIVER_H
 
+#include <debug/debuggable_driver.hpp>
+
 #include "gps_driver.h"
 #include "ubx_datatypes.h"
 
-
-
 namespace xbot::driver::gps {
 class UbxGpsDriver : public GpsDriver {
  public: