102 lines
2.9 KiB
Python
102 lines
2.9 KiB
Python
#
|
|
# Copyright 2010 Nick Foster
|
|
#
|
|
# This file is part of gr-air-modes
|
|
#
|
|
# gr-air-modes is free software; you can redistribute it and/or modify
|
|
# it under the terms of the GNU General Public License as published by
|
|
# the Free Software Foundation; either version 3, or (at your option)
|
|
# any later version.
|
|
#
|
|
# gr-air-modes is distributed in the hope that it will be useful,
|
|
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
# GNU General Public License for more details.
|
|
#
|
|
# You should have received a copy of the GNU General Public License
|
|
# 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.
|
|
#
|
|
|
|
|
|
#!/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:
|
|
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
|
|
else:
|
|
tmp1 = (alt & 0x0FE0) >> 1 #first 7 bits get shifted 1 down but there are only 12 bits in the representation
|
|
tmp2 = 0
|
|
|
|
decoded_alt = ((alt & 0x0F) | tmp1 | tmp2) * 25 - 1000
|
|
|
|
else: #a mode C-style reply
|
|
#okay, the order they come in is:
|
|
#C1 A1 C2 A2 C4 A4 X B1 D1 B2 D2 B4 D4
|
|
#the order we want them in is:
|
|
#D2 D4 A1 A2 A4 B1 B2 B4
|
|
#so we'll reassemble into a Gray-coded representation
|
|
|
|
if bit13 is False:
|
|
alt = (alt & 0x003F) | (alt & 0x0FC0 << 1)
|
|
|
|
C1 = 0x1000
|
|
A1 = 0x0800
|
|
C2 = 0x0400
|
|
A2 = 0x0200 #this represents the order in which the bits come
|
|
C4 = 0x0100
|
|
A4 = 0x0080
|
|
B1 = 0x0020
|
|
D1 = 0x0010
|
|
B2 = 0x0008
|
|
D2 = 0x0004
|
|
B4 = 0x0002
|
|
D4 = 0x0001
|
|
|
|
bigpart = ((alt & B4) >> 1) \
|
|
+ ((alt & B2) >> 2) \
|
|
+ ((alt & B1) >> 3) \
|
|
+ ((alt & A4) >> 4) \
|
|
+ ((alt & A2) >> 5) \
|
|
+ ((alt & A1) >> 6) \
|
|
+ ((alt & D4) << 6) \
|
|
+ ((alt & D2) << 5)
|
|
|
|
#bigpart is now the 500-foot-resolution Gray-coded binary part
|
|
decoded_alt = gray2bin(bigpart)
|
|
#real_alt is now the 500-foot-per-tick altitude
|
|
|
|
cbits = ((alt & C4) >> 8) + ((alt & C2) >> 9) + ((alt & C1) >> 10)
|
|
cval = gray2bin(cbits) #turn them into a real number
|
|
|
|
if cval == 7:
|
|
cval = 5 #not a real gray code after all
|
|
|
|
if decoded_alt % 2:
|
|
cval = 6 - cval #since the code is symmetric this unwraps it to see whether to subtract the C bits or add them
|
|
|
|
decoded_alt *= 500 #take care of the A,B,D data
|
|
decoded_alt += cval * 100 #factor in the C data
|
|
decoded_alt -= 1300 #subtract the offset
|
|
|
|
return decoded_alt
|
|
|
|
def gray2bin(gray):
|
|
i = gray >> 1
|
|
|
|
while i != 0:
|
|
gray ^= i
|
|
i >>= 1
|
|
|
|
return gray
|