diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index e2534ab763..502b3fd7d9 100644 --- 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 index cbeb5004a0..1ddbf871d3 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.h +++ b/libraries/AP_GPS/AP_GPS_ERB.h @@ -133,8 +133,6 @@ private: // 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 index 932fddca2e..43fd72c562 100644 --- 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 index 35258ca4dc..c14aa374de 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -34,8 +34,6 @@ public: // Methods bool read(); - void inject_data(const uint8_t *data, uint16_t len) override; - private: bool parse(uint8_t temp); @@ -81,6 +79,4 @@ private: 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 index 0ad3115215..cb21e9b3df 100644 --- 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 index 74e7ecbe4b..5d574d6144 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -34,8 +34,6 @@ public: // 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 index 5e7c518e49..b4ba25eb1e 100644 --- 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 index aa8d0a9fed..2e9d782772 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -101,8 +101,6 @@ public: 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 index f4bbc515bb..5039d3d9fb 100644 --- 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 index 0e4fca2733..efb09c6cbd 100644 --- 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 @@ public: 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 ; }