diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index b125154d10..3326ac6040 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -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) { diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index bb420b1f59..453e932c2c 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -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); } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 826613747c..9c467a5dfe 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index b70c15af21..dc6246c577 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -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); } diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 5abd60b288..334e847a69 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -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); } diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 3bd80af802..94eea80116 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index b2bf4f4ca7..8638e671eb 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -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,