Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : 
      17             : #include "NMEA.h"
      18             : 
      19             : extern const AP_HAL::HAL &hal;
      20             : 
      21             : /*
      22             :   formatted print of NMEA message to an allocated string, with
      23             :   checksum appended
      24             :  */
      25        1879 : char *nmea_vaprintf(const char *fmt, va_list ap)
      26             : {
      27             :     va_list ap_copy;
      28             : 
      29             :     // we print once to nullptr to get the length
      30        1879 :     va_copy(ap_copy, ap);
      31        1879 :     int len = hal.util->vsnprintf(nullptr, 0, fmt, ap_copy);
      32        1879 :     va_end(ap_copy);
      33        1879 :     if (len <= 0) {
      34             :         // can't print this format
      35           2 :         return nullptr;
      36             :     }
      37             : 
      38             :     // now allocate the right length, including trailer
      39        1877 :     char *s = (char *)malloc(len+6);
      40        1877 :     if (s == nullptr) {
      41             :         // allocation failed
      42           2 :         return nullptr;
      43             :     }
      44             : 
      45        1875 :     if (hal.util->vsnprintf(s, len+5, fmt, ap) < len) {
      46           2 :         free(s);
      47             :         // inconsistent formatting
      48           2 :         return nullptr;
      49             :     }
      50             : 
      51             :     // calculate the checksum
      52        1873 :     uint8_t cs = 0;
      53        1873 :     const uint8_t *b = (const uint8_t *)s+1;
      54       90664 :     while (*b) {
      55       88791 :         cs ^= *b++;
      56             :     }
      57             : 
      58        1873 :     hal.util->snprintf(s+len, 6, "*%02X\r\n", (unsigned)cs);
      59        1873 :     return s;
      60             : }
      61             : 
      62             : /*
      63             :   formatted print of NMEA message to the port, with checksum appended
      64             :  */
      65         106 : bool nmea_printf(AP_HAL::UARTDriver *uart, const char *fmt, ...)
      66             : {
      67             :     va_list ap;
      68             : 
      69         106 :     va_start(ap, fmt);
      70         106 :     char *s = nmea_vaprintf(fmt, ap);
      71         106 :     va_end(ap);
      72         106 :     if (s == nullptr) {
      73           6 :         return false;
      74             :     }
      75             : 
      76         100 :     size_t len = strlen(s);
      77         100 :     if (uart->txspace() < len) {
      78           2 :         free(s);
      79           2 :         return false;
      80             :     }
      81          98 :     uart->write((const uint8_t*)s, len);
      82          98 :     free(s);
      83          98 :     return true;
      84             : }
      85             : 
      86             : 
      87             : /*
      88             :   formatted print of NMEA message to a buffer, with checksum appended.
      89             :   Returns the length of the string filled into buf. If the NMEA string does not fit in the buffer, returns 0
      90             :  */
      91         152 : uint16_t nmea_printf_buffer(char* buf, const uint16_t buf_max_len, const char *fmt, ...)
      92             : {
      93             :     va_list ap;
      94             : 
      95         152 :     va_start(ap, fmt);
      96         152 :     char *s = nmea_vaprintf(fmt, ap);
      97         152 :     va_end(ap);
      98         152 :     if (s == nullptr) {
      99           0 :         return 0;
     100             :     }
     101             : 
     102         152 :     size_t len = strlen(s);
     103         152 :     if (len > buf_max_len) {
     104             :         // our string is larger than the buffer we've supplied.
     105             :         // Instead of populating the buffer with a partial message, just quietly fail and do nothing
     106           0 :         len = 0;
     107             :     } else {
     108         152 :         strncpy(buf, s, buf_max_len);
     109             :     }
     110         152 :     free(s);
     111             :     
     112         152 :     return len;
     113             : }