I'm recieving two bytes of data on a serial interface.

They are structured as follows and I'm wondering about the most PBPro code size efficent way of converting them into meaningful data?

'Each trouble code requires 2 bytes to describe.
'The text description of a trouble code may be decoded as follows.
'The first character in the trouble code is determined by the first two bits in the first byte:

'A7 A6 First DTC character
'-- -- -------------------
' 0 0 P - Powertrain
' 0 1 C - Chassis
' 1 0 B - Body
' 1 1 U - Network

'The four following digits are BCD encoded.

'The second character in the DTC is a number defined by

'A5 A4 Second DTC character
'-- -- --------------------
' 0 0 0
' 0 1 1
' 1 0 2
' 1 1 3

'The third character in the DTC is a number defined by

'A3 A2 A1 A0 Third DTC character
'-- -- -- -- -------------------
' 0 0 0 0 0
' 0 0 0 1 1
' 0 0 1 0 2
' 0 0 1 1 3
' 0 1 0 0 4
' 0 1 0 1 5
' 0 1 1 0 6
' 0 1 1 1 7
' 1 0 0 0 8
' 1 0 0 1 9

'The fourth and fifth characters are defined in the same way as the third,
'but using bits B7..B4 and B3..B0. The resulting five-character code should
'look something like "U0158" and can be looked up in a table of OBD-II DTCs.
Anyone done this before and have any sample code?

I could do it with If/Then and bit testing but that didn't sound very efficient?

BCD maths is not my strong point.