AP_Common: Make hexadecimal character number conversion method common

libraries/AP_Common/AP_Common.cpp

@@ -36,3 +36,37 @@ bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
     return false;
 
 }
+
+/**
+ * return the numeric value of an ascii hex character
+ *
+ * @param [in]  a   Hexa character
+ * @param [out] res uint8 value
+ * @retval true  Conversion OK
+ * @retval false Input value error
+ * @Note Input character is 0-9, A-F, a-f
+ *  A 0x41, a 0x61, 0 0x30
+ */
+bool hex_to_uint8(uint8_t a, uint8_t &res)
+{
+    uint8_t nibble_low  = a & 0xf;
+
+    switch (a & 0xf0) {
+    case 0x30:  // 0-
+        if (nibble_low > 9) {
+            return false;
+        }
+        res = nibble_low;
+        break;
+    case 0x40:  // uppercase A-
+    case 0x60:  // lowercase a-
+        if (nibble_low == 0 || nibble_low > 6) {
+            return false;
+        }
+        res = nibble_low + 9;
+        break;
+    default:
+        return false;
+    }
+    return true;
+}

libraries/AP_Common/AP_Common.h

@@ -133,3 +133,5 @@ template<typename s, size_t t> struct assert_storage_size {
   False otherwise.
 */
 bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound);
+
+bool hex_to_uint8(uint8_t a, uint8_t &res);  // return the uint8 value of an ascii hex character

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;
@@ -234,7 +221,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();

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -82,13 +82,6 @@ class AP_GPS_NMEA : public AP_GPS_Backend
     ///
     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.
     ///

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -134,7 +134,12 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
 {
     // handle the last term in a message
     if (_term_is_checksum) {
-        uint8_t checksum = 16 * char_to_hex(_term[0]) + char_to_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;
         return ((checksum == _checksum) &&
                 !is_negative(_distance_m) &&
                 (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT));
@@ -174,14 +179,3 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
 
     return false;
 }
-
-// return the numeric value of an ascii hex character
-int16_t AP_RangeFinder_NMEA::char_to_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';
-}

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -61,9 +61,6 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend
     // returns true if new sentence has just passed checksum test and is validated
     bool decode_latest_term();
 
-    // return the numeric value of an ascii hex character
-    static int16_t char_to_hex(char a);
-
     AP_HAL::UARTDriver *uart = nullptr;     // pointer to serial uart
 
     // message decoding related members