Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: use common nmea_printf()

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

25
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -19,6 +19,7 @@
#include "UARTDriver.h" #include "UARTDriver.h"
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_GPS/AP_GPS_UBLOX.h> #include <AP_GPS/AP_GPS_UBLOX.h>
#include <AP_Common/NMEA.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <unistd.h> #include <unistd.h>
#include <time.h> #include <time.h>
@ -652,39 +653,21 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d, uint8_t instance)
_gps_write((uint8_t*)&p, sizeof(p), 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 formatted print of NMEA message, with checksum appended
*/ */
void SITL_State::_gps_nmea_printf(uint8_t instance, const char *fmt, ...) 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_list ap;
va_start(ap, fmt); va_start(ap, fmt);
vasprintf(&s, fmt, ap); char *s = nmea_vaprintf(fmt, ap);
va_end(ap); va_end(ap);
csum = _gps_nmea_checksum(s); if (s != nullptr) {
snprintf(trailer, sizeof(trailer), "*%02X\r\n", (unsigned)csum);
_gps_write((const uint8_t*)s, strlen(s), instance); _gps_write((const uint8_t*)s, strlen(s), instance);
_gps_write((const uint8_t*)trailer, 5, instance);
free(s); free(s);
} }
}
/* /*