diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 4978a54db1..fd4ecbc7e9 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -19,6 +19,7 @@ #include "UARTDriver.h" #include #include +#include #include #include #include @@ -652,38 +653,20 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d, uint8_t instance) _gps_write((uint8_t*)&p, sizeof(p), instance); } -/* - NMEA checksum - */ -uint8_t SITL_State::_gps_nmea_checksum(const char *s) -{ - uint8_t cs = 0; - const uint8_t *b = (const uint8_t *)s; - for (uint16_t i=1; s[i]; i++) { - cs ^= b[i]; - } - return cs; -} - /* formatted print of NMEA message, with checksum appended */ void SITL_State::_gps_nmea_printf(uint8_t instance, const char *fmt, ...) { - char *s = nullptr; - uint8_t csum; - char trailer[6]; - va_list ap; va_start(ap, fmt); - vasprintf(&s, fmt, ap); + char *s = nmea_vaprintf(fmt, ap); va_end(ap); - csum = _gps_nmea_checksum(s); - snprintf(trailer, sizeof(trailer), "*%02X\r\n", (unsigned)csum); - _gps_write((const uint8_t*)s, strlen(s), instance); - _gps_write((const uint8_t*)trailer, 5, instance); - free(s); + if (s != nullptr) { + _gps_write((const uint8_t*)s, strlen(s), instance); + free(s); + } }