From e4455491aa3c48e91a30c95bd62dd6ea0090641f Mon Sep 17 00:00:00 2001 From: murata Date: Fri, 12 Jul 2019 10:52:38 +0200 Subject: [PATCH] AP_GPS: Use common hexadecimal character to number conversion method --- libraries/AP_GPS/AP_GPS_NMEA.cpp | 20 ++++++-------------- libraries/AP_GPS/AP_GPS_NMEA.h | 7 ------- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index c4fcdde8fb..5e0631ca2c 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -115,19 +115,6 @@ bool AP_GPS_NMEA::_decode(char c) return valid_sentence; } -// -// internal utilities -// -int16_t AP_GPS_NMEA::_from_hex(char a) -{ - if (a >= 'A' && a <= 'F') - return a - 'A' + 10; - else if (a >= 'a' && a <= 'f') - return a - 'a' + 10; - else - return a - '0'; -} - int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p) { char *endptr = nullptr; @@ -240,7 +227,12 @@ bool AP_GPS_NMEA::_term_complete() { // handle the last term in a message if (_is_checksum_term) { - uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]); + uint8_t nibble_high = 0; + uint8_t nibble_low = 0; + if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) { + return false; + } + const uint8_t checksum = (nibble_high << 4u) | nibble_low; if (checksum == _parity) { if (_gps_data_good) { uint32_t now = AP_HAL::millis(); diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index a7725950a3..7bc9a7c5cd 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -83,13 +83,6 @@ private: /// bool _decode(char c); - /// Return the numeric value of an ascii hex character - /// - /// @param a The character to be converted - /// @returns The value of the character as a hex digit - /// - int16_t _from_hex(char a); - /// Parses the @p as a NMEA-style decimal number with /// up to 3 decimal digits. ///