diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index c969ccc8b4..8235c68605 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -29,6 +29,7 @@ /// #include +#include #include #include @@ -75,41 +76,6 @@ bool AP_GPS_NMEA::read(void) return parsed; } -/* - formatted print of NMEA message to the port, with checksum appended - */ -bool AP_GPS_NMEA::nmea_printf(const char *fmt, ...) const -{ - char *s = nullptr; - char trailer[6]; - va_list ap; - - va_start(ap, fmt); - int ret = vasprintf(&s, fmt, ap); - va_end(ap); - if (ret == -1 || s == nullptr) { - // allocation failed - return false; - } - - // calculate the checksum - uint8_t cs = 0; - const uint8_t *b = (const uint8_t *)s+1; - while (*b) { - cs ^= *b++; - } - uint32_t len = strlen(s); - snprintf(trailer, sizeof(trailer), "*%02X\r\n", (unsigned)cs); - if (port->txspace() < len + 5) { - free(s); - return false; - } - port->write((const uint8_t*)s, len); - port->write((const uint8_t*)trailer, 5); - free(s); - return true; -} - bool AP_GPS_NMEA::_decode(char c) { bool valid_sentence = false; @@ -279,7 +245,7 @@ bool AP_GPS_NMEA::_have_new_message() const int32_t dt_ms = now - _last_fix_ms; if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 && get_type() == AP_GPS::GPS_TYPE_ALLYSTAR) { - nmea_printf("$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0", + nmea_printf(port, "$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0", 1000U/gps._rate_ms[state.instance], gps._rate_ms[state.instance]); } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index a90840c575..0d26ad5987 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -115,9 +115,6 @@ private: /// return true if we have a new set of NMEA messages bool _have_new_message(void); - // print a formatted NMEA message to the port - bool nmea_printf(const char *fmt, ...) const; - uint8_t _parity; ///< NMEA message checksum accumulator bool _is_checksum_term; ///< current term is the checksum char _term[15]; ///< buffer for the current term within the current sentence