diff --git a/pyModeS/modes_common.py b/pyModeS/modes_common.py
index b56e3c0..a541356 100644
--- a/pyModeS/modes_common.py
+++ b/pyModeS/modes_common.py
@@ -95,29 +95,37 @@ def altcode(msg):
             A4 = mbin[24]
             # _ = mbin[25]
             B1 = mbin[26]
-            # _ = mbin[27]
+            # D1 = mbin[27]     # always zero
             B2 = mbin[28]
             D2 = mbin[29]
             B4 = mbin[30]
             D4 = mbin[31]
 
-            # standard greycode
-            gc5 = D2 + D4 + A1 + A2 + A4 + B1 + B2 + B4
-            N5 = int(util.gray2bin(gc5, 8), 2)
+            greystr =  D2 + D4 + A1 + A2 + A4 + B1 + B2 + B4 + C1 + C2 + C4
+            alt = grey2alt(greystr)
 
-            # in 100-ft step must be converted
-            gc1 = C1 + C2 + C4
-            N1 = int(util.gray2bin(gc1, 3), 2) - 1
-
-            if N1 == 6:
-                N1 = 4
-
-            if N5%2 != 0:
-                N1 = 4 - N1
-
-            alt = (N5*500 + N1*100) - 1200
     if mbit == '1':         # unit in meter
         vbin = mbin[19:25] + mbin[26:31]
         alt = int(util.bin2int(vbin) * 3.28084)  # convert to ft
 
     return alt
+
+def grey2alt(codestr):
+    gc500 = codestr[:8]
+    n500 = util.gray2int(gc500)
+
+    # in 100-ft step must be converted first
+    gc100 = codestr[8:]
+    n100 = util.gray2int(gc100)
+
+    if n100 in [0, 5, 6]:
+        return None
+
+    if n100 == 7:
+        n100 = 5
+
+    if n500%2:
+        n100 = 6 - n100
+
+    alt = (n500*500 + n100*100) - 1300
+    return alt
diff --git a/pyModeS/util.py b/pyModeS/util.py
index 8dab2d7..25b5b15 100644
--- a/pyModeS/util.py
+++ b/pyModeS/util.py
@@ -87,14 +87,11 @@ def floor(x):
     return int(math.floor(x))
 
 
-def bin2gray(binary, nbits):
-    """Convert binary to greycode"""
-    graycode = binary
-    for i in range(1, nbits):
-        bit = str(int(binary[i-1]) ^ int(binary[i]))
-        graycode = str(graycode[:i]) + str(bit)
-    return graycode
-
-def gray2bin(greycode, nbits):
-    """Convert greycode to binary"""
-    return bin2gray(greycode, nbits)    # simply XOR again
+def gray2int(graystr):
+    """Convert greycode to binary (DF4, 20 altitude coding)"""
+    num = bin2int(graystr)
+    num ^= (num >> 8)
+    num ^= (num >> 4)
+    num ^= (num >> 2)
+    num ^= (num >> 1)
+    return num
diff --git a/tests/test_ehs.py b/tests/test_ehs.py
index ea06513..d14a7dc 100644
--- a/tests/test_ehs.py
+++ b/tests/test_ehs.py
@@ -1,5 +1,5 @@
 from pyModeS import ehs
-
+from pyModeS import modes_common
 
 def test_ehs_icao():
     assert ehs.icao("A0001839CA3800315800007448D9") == '400940'
@@ -49,3 +49,20 @@ def test_ehs_BDS60_functions():
     assert ehs.mach60("A000029CFFBAA11E2004727281F1") == 0.48
     assert ehs.vr60baro("A000029CFFBAA11E2004727281F1") == 0
     assert ehs.vr60ins("A000029CFFBAA11E2004727281F1") == 3648
+
+def test_greycode_to_altitude():
+    assert modes_common.grey2alt('00000000010') == -1000
+    assert modes_common.grey2alt('00000001010') == -500
+    assert modes_common.grey2alt('00000011011') == -100
+    assert modes_common.grey2alt('00000011010') == 0
+    assert modes_common.grey2alt('00000011110') == 100
+    assert modes_common.grey2alt('00000010011') == 600
+    assert modes_common.grey2alt('00000110010') == 1000
+    assert modes_common.grey2alt('00001001001') == 5800
+    assert modes_common.grey2alt('00011100100') == 10300
+    assert modes_common.grey2alt('01100011010') == 32000
+    assert modes_common.grey2alt('01110000100') == 46300
+    assert modes_common.grey2alt('01010101100') == 50200
+    assert modes_common.grey2alt('11011110100') == 73200
+    assert modes_common.grey2alt('10000000011') == 126600
+    assert modes_common.grey2alt('10000000001') == 126700