From 00d006a3ab94e621d502e8a7bad874c5cedb3151 Mon Sep 17 00:00:00 2001
From: Nick Foster <nick@ettus.com>
Date: Fri, 18 May 2012 14:43:01 -0700
Subject: [PATCH] Fix altitude decoding and implement altitude encoding. Test
 case which loops back altitude.

---
 python/altitude.py | 60 +++++++++++++++++++++++++++++++++++-----------
 1 file changed, 46 insertions(+), 14 deletions(-)
 mode change 100644 => 100755 python/altitude.py

diff --git a/python/altitude.py b/python/altitude.py
old mode 100644
new mode 100755
index 53b64d4..3d55d3f
--- a/python/altitude.py
+++ b/python/altitude.py
@@ -1,5 +1,6 @@
+#!/usr/bin/env python
 #
-# Copyright 2010 Nick Foster
+# Copyright 2010, 2012 Nick Foster
 # 
 # This file is part of gr-air-modes
 # 
@@ -17,25 +18,30 @@
 # along with gr-air-modes; see the file COPYING.  If not, write to
 # the Free Software Foundation, Inc., 51 Franklin Street,
 # Boston, MA 02110-1301, USA.
-# 
+#
 
+# For reference into the methodology used to decode altitude,
+# see RTCA DO-181D p.42
 
-#!/usr/bin/env python
-#from string import split, join
-
-#betcha this would be faster if you used a table for mode C
-#you could strip out D1 since it's never used, that leaves 11 bits (table is 2048 entries)
-#on the other hand doing it this way is educational for others
 def decode_alt(alt, bit13):
-	if alt & 0x40 and bit13 is True:
+	mbit = alt & 0x0040
+	qbit = alt & 0x0010
+	
+	if mbit and bit13:
+		#TBD: bits 20-25, 27-31 encode alt in meters
+		#remember that bits are left justified (bit 20 is MSB)
 		return "METRIC ERROR"
 
-	if alt & 0x10: #a mode S-style reply
-		if bit13 is True:
-			tmp1 = (alt & 0x1F80) >> 2 #first 6 bits get shifted 2 down
-			tmp2 = (alt & 0x20) >> 1 #that bit gets shifted 1 down
+	if qbit: #a mode S-style reply
+		#bit13 is false for BDS0,5 ADS-B squitters, and is true otherwise
+		if bit13:
+			#in this representation, the altitude bits are as follows:
+			# 12 11 10 9 8 7 (6) 5 (4) 3 2 1 0
+			# so bits 6 and 4 are the M and Q bits, respectively.
+			tmp1 = (alt & 0x3F80) >> 2
+			tmp2 = (alt & 0x0020) >> 1
 		else:
-			tmp1 = (alt & 0x0FE0) >> 1 #first 7 bits get shifted 1 down but there are only 12 bits in the representation
+			tmp1 = (alt & 0x1FE0) >> 1
 			tmp2 = 0
 
 		decoded_alt = ((alt & 0x0F) | tmp1 | tmp2) * 25 - 1000
@@ -99,3 +105,29 @@ def gray2bin(gray):
 		i >>= 1
 
 	return gray
+
+def encode_alt_modes(alt, bit13):
+	mbit = False
+	qbit = True
+	encalt = (int(alt) + 1000) / 25
+
+	if bit13 is True:
+		tmp1 = (encalt & 0xfe0) << 2
+		tmp2 = (encalt & 0x010) << 1
+		
+	else:
+		tmp1 = (encalt & 0xff8) << 1
+		tmp2 = 0
+
+	return (encalt & 0x0F) | tmp1 | tmp2 | (mbit << 6) | (qbit << 4)
+
+if __name__ == "__main__":
+	for alt in range(-1000, 101400, 25):
+		dec = decode_alt(encode_alt_modes(alt, False), False)
+		if dec != alt:
+			print "Failure at %i with bit13 clear (got %s)" % (alt, dec)
+	for alt in range(-1000, 101400, 25):
+		dec = decode_alt(encode_alt_modes(alt, True), True)
+		if dec != alt:
+			print "Failure at %i with bit13 set (got %s)" % (alt, dec)
+