libraries/AP_Common/NMEA.cpp - master - Sangak/ardupilot

libraries/AP_Common/NMEA.cpp
113 строк3 KB
Tom Pittenger
AP_Common: add NMEA output to a buffer
07 фев 2023, 13:12
07 фев 2023, 13:12ea35b28
/*
   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;
}


/*
  formatted print of NMEA message to a buffer, with checksum appended.
  Returns the length of the string filled into buf. If the NMEA string does not fit in the buffer, returns 0
 */
uint16_t nmea_printf_buffer(char* buf, const uint16_t buf_max_len, const char *fmt, ...)
{
    va_list ap;

    va_start(ap, fmt);
    char *s = nmea_vaprintf(fmt, ap);
    va_end(ap);
    if (s == nullptr) {
        return 0;
    }

    size_t len = strlen(s);
    if (len > buf_max_len) {
        // our string is larger than the buffer we've supplied.
        // Instead of populating the buffer with a partial message, just quietly fail and do nothing
        len = 0;
    } else {
        strncpy(buf, s, buf_max_len);
    }
    free(s);
    
    return len;
}