aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: use common nmea_printf()

Browse Source
This commit is contained in:
Andrew Tridgell 2021-02-06 17:23:45 +11:00
parent 6c29f8c24f
commit 3650c02745
2 changed files with 2 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -29,6 +29,7 @@
/// ///
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Common/NMEA.h>
#include <ctype.h> #include <ctype.h>
#include <stdint.h> #include <stdint.h>
@ -75,41 +76,6 @@ bool AP_GPS_NMEA::read(void)
return parsed; 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 AP_GPS_NMEA::_decode(char c)
{ {
bool valid_sentence = false; bool valid_sentence = false;
@ -279,7 +245,7 @@ bool AP_GPS_NMEA::_have_new_message()
const int32_t dt_ms = now - _last_fix_ms; const int32_t dt_ms = now - _last_fix_ms;
if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 && if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 &&
get_type() == AP_GPS::GPS_TYPE_ALLYSTAR) { 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], 1000U/gps._rate_ms[state.instance],
gps._rate_ms[state.instance]); gps._rate_ms[state.instance]);
} }

3
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -115,9 +115,6 @@ private:
/// return true if we have a new set of NMEA messages /// return true if we have a new set of NMEA messages
bool _have_new_message(void); 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 uint8_t _parity; ///< NMEA message checksum accumulator
bool _is_checksum_term; ///< current term is the checksum bool _is_checksum_term; ///< current term is the checksum
char _term[15]; ///< buffer for the current term within the current sentence char _term[15]; ///< buffer for the current term within the current sentence