AP_GPS: added logging to more serial GPS backends

This commit is contained in:
Andrew Tridgell 2022-10-15 11:10:36 +11:00 committed by Randy Mackay
parent 12a273f376
commit b0d0575906
7 changed files with 21 additions and 3 deletions

View File

@ -57,6 +57,9 @@ AP_GPS_ERB::read(void)
// read the next byte
data = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&data, 1);
#endif
reset:
switch(_step) {

View File

@ -76,6 +76,9 @@ AP_GPS_GSOF::read(void)
bool ret = false;
while (port->available() > 0) {
uint8_t temp = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&temp, 1);
#endif
ret |= parse(temp);
}

View File

@ -54,12 +54,12 @@ bool AP_GPS_NMEA::read(void)
numc = port->available();
while (numc--) {
char c = port->read();
if (_decode(c)) {
parsed = true;
}
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data((const uint8_t *)&c, 1);
#endif
if (_decode(c)) {
parsed = true;
}
}
return parsed;
}

View File

@ -90,6 +90,9 @@ AP_GPS_NOVA::read(void)
bool ret = false;
while (port->available() > 0) {
uint8_t temp = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&temp, 1);
#endif
ret |= parse(temp);
}

View File

@ -86,6 +86,9 @@ AP_GPS_SBF::read(void)
uint32_t available_bytes = port->available();
for (uint32_t i = 0; i < available_bytes; i++) {
uint8_t temp = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&temp, 1);
#endif
ret |= parse(temp);
}

View File

@ -100,6 +100,9 @@ AP_GPS_SBP::_sbp_process()
while (port->available() > 0) {
uint8_t temp = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&temp, 1);
#endif
uint16_t crc;

View File

@ -103,6 +103,9 @@ AP_GPS_SBP2::_sbp_process()
while (nleft > 0) {
nleft--;
uint8_t temp = port->read();
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&temp, 1);
#endif
uint16_t crc;
//This switch reads one character at a time,