From 6c29f8c24fc271403988920d9e5cf6b94c81aa77 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell <andrew@tridgell.net> Date: Sat, 6 Feb 2021 17:23:19 +1100 Subject: [PATCH] AP_Common: added common nmea_printf code --- libraries/AP_Common/NMEA.cpp | 84 ++++++++++++++++++++++++++++++++++++ libraries/AP_Common/NMEA.h | 27 ++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 libraries/AP_Common/NMEA.cpp create mode 100644 libraries/AP_Common/NMEA.h diff --git a/libraries/AP_Common/NMEA.cpp b/libraries/AP_Common/NMEA.cpp new file mode 100644 index 0000000000..b942644dd3 --- /dev/null +++ b/libraries/AP_Common/NMEA.cpp @@ -0,0 +1,84 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + + +#include "NMEA.h" + +extern const AP_HAL::HAL &hal; + +/* + formatted print of NMEA message to an allocated string, with + checksum appended + */ +char *nmea_vaprintf(const char *fmt, va_list ap) +{ + va_list ap_copy; + + // we print once to nullptr to get the length + va_copy(ap_copy, ap); + int len = hal.util->vsnprintf(nullptr, 0, fmt, ap_copy); + va_end(ap_copy); + if (len <= 0) { + // can't print this format + return nullptr; + } + + // now allocate the right length, including trailer + char *s = (char *)malloc(len+6); + if (s == nullptr) { + // allocation failed + return nullptr; + } + + if (hal.util->vsnprintf(s, len+5, fmt, ap) < len) { + free(s); + // inconsistent formatting + return nullptr; + } + + // calculate the checksum + uint8_t cs = 0; + const uint8_t *b = (const uint8_t *)s+1; + while (*b) { + cs ^= *b++; + } + + hal.util->snprintf(s+len, 6, "*%02X\r\n", (unsigned)cs); + return s; +} + +/* + formatted print of NMEA message to the port, with checksum appended + */ +bool nmea_printf(AP_HAL::UARTDriver *uart, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + char *s = nmea_vaprintf(fmt, ap); + va_end(ap); + if (s == nullptr) { + return false; + } + + size_t len = strlen(s); + if (uart->txspace() < len) { + free(s); + return false; + } + uart->write((const uint8_t*)s, len); + free(s); + return true; +} diff --git a/libraries/AP_Common/NMEA.h b/libraries/AP_Common/NMEA.h new file mode 100644 index 0000000000..3207706a68 --- /dev/null +++ b/libraries/AP_Common/NMEA.h @@ -0,0 +1,27 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <AP_HAL/AP_HAL.h> + +/* + formatted print of NMEA message to an allocated string, with + checksum appended + */ +char *nmea_vaprintf(const char *fmt, va_list ap); + +/* + formatted print of NMEA message to a uart, with checksum appended + */ +bool nmea_printf(AP_HAL::UARTDriver *uart, const char *fmt, ...);