diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index 217841cb39..94cdc4f10f 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/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; +} diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index d6fb0f5890..8883bd164d 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -133,3 +133,5 @@ template 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