AP_GPS: Unify inject_data interface for all classes

Yukong · Apr 19, 2017 · 6f297de · 6f297de
1 parent 88e02c7
commit 6f297de
Show file tree

Hide file tree

Showing 10 changed files with 24 additions and 55 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp
@@ -223,17 +223,6 @@ AP_GPS_ERB::_parse_gps(void)
     return false;
 }
 
-void
-AP_GPS_ERB::inject_data(const uint8_t *data, uint16_t len)
-{
-
-    if (port->txspace() > len) {
-        port->write(data, len);
-    } else {
-        Debug("ERB: Not enough TXSPACE");
-    }
-}
-
 /*
   detect a ERB GPS. Adds one byte, and returns true if the stream
   matches a ERB

diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h
@@ -133,8 +133,6 @@ class AP_GPS_ERB : public AP_GPS_Backend
     // Buffer parse & GPS state update
     bool _parse_gps();
 
-    void inject_data(const uint8_t *data, uint16_t len) override;
-
     // used to update fix between status and position packets
     AP_GPS::GPS_Status next_fix;
 };
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp
@@ -342,13 +342,3 @@ AP_GPS_GSOF::process_message(void)
     return false;
 }
 
-void
-AP_GPS_GSOF::inject_data(const uint8_t *data, uint16_t len)
-{
-    if (port->txspace() > len) {
-        last_injected_data_ms = AP_HAL::millis();
-        port->write(data, len);
-    } else {
-        Debug("GSOF: Not enough TXSPACE");
-    }
-}
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h
@@ -34,8 +34,6 @@ class AP_GPS_GSOF : public AP_GPS_Backend
     // Methods
     bool read();
 
-    void inject_data(const uint8_t *data, uint16_t len) override;
-
 private:
 
     bool parse(uint8_t temp);
@@ -81,6 +79,4 @@ class AP_GPS_GSOF : public AP_GPS_Backend
     uint32_t gsofmsg_time = 0;
     uint8_t gsofmsgreq_index = 0;
     uint8_t gsofmsgreq[5] = {1,2,8,9,12};
-
-    uint32_t last_injected_data_ms = 0;
 };
diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -292,14 +292,3 @@ AP_GPS_SBF::process_message(void)
     return false;
 }
 
-void
-AP_GPS_SBF::inject_data(const uint8_t *data, uint16_t len)
-{
-
-    if (port->txspace() > len) {
-        last_injected_data_ms = AP_HAL::millis();
-        port->write(data, len);
-    } else {
-        Debug("SBF: Not enough TXSPACE");
-    }
-}
diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h
@@ -34,8 +34,6 @@ class AP_GPS_SBF : public AP_GPS_Backend
     // Methods
     bool read();
 
-    void inject_data(const uint8_t *data, uint16_t len) override;
-
 private:
 
     bool parse(uint8_t temp);

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -1275,17 +1275,6 @@ AP_GPS_UBLOX::_configure_rate(void)
     _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
 }
 
-void
-AP_GPS_UBLOX::inject_data(const uint8_t *data, uint16_t len)
-{
-    if (port->txspace() > len) {
-        port->write(data, len);
-    } else {
-        Debug("UBX: Not enough TXSPACE");
-    }
-}
-
-
 static const char *reasons[] = {"navigation rate",
                                 "posllh rate",
                                 "status rate",

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -101,8 +101,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
 
-    void inject_data(const uint8_t *data, uint16_t len) override;
-
     bool is_configured(void) {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
         if (!gps._auto_config) {

diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
@@ -16,6 +16,14 @@
 #include "AP_GPS.h"
 #include "GPS_Backend.h"
 
+#define GPS_BACKEND_DEBUGGING 0
+
+#if GPS_BACKEND_DEBUGGING
+ # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#else
+ # define Debug(fmt, args ...)
+#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) :
@@ -112,3 +120,17 @@ void AP_GPS_Backend::fill_3d_velocity(void)
     state.velocity.z = 0;
     state.have_vertical_velocity = false;
 }
+
+void
+AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
+{
+    // not all backends have valid ports
+    if (port != nullptr) {
+        if (port->txspace() > len) {
+            port->write(data, len);
+        } else {
+            Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
+        }
+    }
+}
+
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
@@ -24,7 +24,7 @@
 class AP_GPS_Backend
 {
 public:
-	AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
+    AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
 
     // we declare a virtual destructor so that GPS drivers can
     // override with a custom destructor if need be.
@@ -41,7 +41,7 @@ class AP_GPS_Backend
 
     virtual bool is_configured(void) { return true; }
 
-    virtual void inject_data(const uint8_t *data, uint16_t len) { return; }
+    virtual void inject_data(const uint8_t *data, uint16_t len);
 
     //MAVLink methods
     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }