gr-air-modes/python/modes_parse.py

#
# Copyright 2010, 2012 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.
#

import time, os, sys
from string import split, join
from altitude import decode_alt
import cpr
import math
from modes_exceptions import *

#this implements a packet class which can retrieve its own fields.
class data_field:
  def __init__(self, data):
    self.data = data

  fields = { }
  types = { }
  subfields = { } #fields to return objects instead of just returning bits,
                  #used for fields which may have multiple meanings (like ME for Type 17)
  offset = 1   #field offset applied to all fields. used for offsetting
               #subtypes to reconcile with the spec. Really just for readability.

  #get a particular field from the data
  def __getitem__(self, fieldname):
    if self.get_type() in self.types:
      if fieldname in self.types[self.get_type()]: #verify it exists in this packet type
        if fieldname in self.subfields:
          #create a new subfield object and return it
          return self.subfields[fieldname](self.get_bits(self.fields[fieldname]))
        else:
          return self.get_bits(self.fields[fieldname])
      else:
        raise FieldNotInPacket(fieldname)
    else:
      raise NoHandlerError(self.get_type())

  #grab all the fields in the packet as a dict
  def get_fields(self):
    return {field: self[field] for field in self.types[self.get_type()]}

  def get_type(self):
    raise NotImplementedError

  def get_numbits(self):
    raise NotImplementedError

  #retrieve bits from data given the offset and number of bits.
  #the offset is both left-justified (LSB) and starts at 1, to
  #correspond to the Mode S spec. Blame them.
  def get_bits(self, arg):
    (startbit, num) = arg
    return (self.data \
        >> (self.get_numbits() - startbit - num + self.offset)) \
         & ((1 << num) - 1)

#type MB (extended squitter types 20,21) subfields
class mb_reply(data_field):
  fields = { "acs": (45,20), "ais": (41,48),  "ara": (41,14), "bcs": (65,16),
             "bds": (33,8),  "bds1": (33,4),  "bds2": (37,4), "cfs": (41,4),
             "ecs": (81,8),  "mte": (60,1),   "rac": (55,4),  "rat": (59,1),
             "tid": (33,26), "tida": (63,13), "tidb": (83,6), "tidr": (76,7),
             "tti": (61,2)
           }
  offset = 33 #fields offset by 33 to match documentation

  #types are based on bds1 subfield
  types = { 0: ["bds", "bds1", "bds2"], #TODO
            1: ["bds", "bds1", "bds2", "cfs", "acs", "bcs"],
            2: ["bds", "bds1", "bds2", "ais"],
            3: ["bds", "bds1", "bds2", "ara",  "rac", "rat",
                "mte", "tti",  "tida", "tidr", "tidb"]
          }

  def get_type(self):
    bds1 = self.get_bits(self.fields["bds1"])
    bds2 = self.get_bits(self.fields["bds2"])
    if bds1 not in (0,1,2,3) or bds2 not in (0,):
      raise NoHandlerError
    return int(bds1)

  def get_numbits(self):
    return 56

class bds09_reply(data_field):
  fields = {
             "sub":  (6,3),   "dew":  (10,1),  "vew":  (11,11), "dns":   (22,1),
             "vns":  (23,11), "str":  (34,1),  "tr":    (35,6),   "dvr":  (41,1),
             "vr":   (42,9),  "icf":  (9,1),   "ifr":   (10,1),   "nuc":  (11,3),
             "gdew": (14,1),  "gvew": (15,10), "gdns":  (25,1),   "gvns": (26,10),
             "vrsrc":  (36,1),  "gdvr": (37,1),  "gvr":   (38,9),   "ghds": (49,1),
             "ghd":  (50,6),  "mhs":  (14,1),  "hdg":   (15,10),  "ast":  (25,1),
             "spd":  (26,10)
           }

  offset = 6
           
  types = {
             #BDS0,9 subtype 0
             0: ["sub", "dew", "vew", "dns", "vns", "str", "tr", "dvr", "vr"],
             #BDS0,9 subtypes 1-2 (differ only in velocity encoding)
             1: ["sub", "icf", "ifr", "nuc", "gdew", "gvew", "gdns", "gvns", "vrsrc", "gdvr", "gvr", "ghds", "ghd"],
             #BDS0,9 subtype 3-4 (airspeed and heading)
             3: ["sub", "icf", "ifr", "nuc", "mhs", "hdg", "ast", "spd", "vrs", "gsvr", "gvr", "ghds", "ghd"]
          }

  def get_type(self):
    sub = self.get_bits(self.fields["sub"])
    if sub == 0:
      return 0
    if 1 <= sub <= 2:
      return 1
    if 3 <= sub <= 4:
      return 3

  def get_numbits(self):
    return 51
      
#type 17 extended squitter data
class me_reply(data_field):
  #TODO: add comments explaining these fields
  fields = { "ftc":  (1,5),
             #BDS0,5 and BDS0,6 position information fields
             "ss":   (6,2),   "saf":  (8,1),  "alt":    (9, 12),
             "time": (21,1),  "cpr":  (22,1),  "lat":   (23, 17), "lon":  (40, 17),
             "mvt":  (6,7),   "gts":  (13,1),  "gtk":   (14,7),   "trs":  (1,2),
             "ats":  (3,1),   "cat":  (6,3),   "ident": (9,48),
             #subclassed BDS0,9 velocity information subfields
             "bds09": (6,51),
             #emergency type BDS6,1
             "eps":  (9,3)
             #TODO: TCP, TCP+1/BDS 6,2/FTC 29
           }

  subfields = { "bds09": bds09_reply }

  #types in this format are listed by BDS register
  types = { 0x05: ["ftc", "ss", "saf", "alt", "time", "cpr", "lat", "lon"], #airborne position
            0x06: ["ftc", "mvt", "gts", "gtk", "time", "cpr", "lat", "lon"], #surface position
            0x07: ["ftc",], #TODO extended squitter status
            0x08: ["ftc", "cat", "ident"], #extended squitter identification and type
            0x09: ["ftc", "bds09"],
            #0x0A: data link capability report
            #0x17: common usage capability report
            #0x18-0x1F: Mode S specific services capability report
            #0x20: aircraft identification
            0x61: ["ftc", "eps"]
          }

  #maps ftc to BDS register
  def get_type(self):
    ftc = self.get_bits(self.fields["ftc"])
    if 1 <= ftc <= 4:
      return 0x08
    elif 5 <= ftc <= 8:
      return 0x06
    elif 9 <= ftc <= 18:
      return 0x05
    elif ftc == 19:
      return 0x09
    else:
      return NoHandlerError
    
  def get_numbits(self):
    return 56

class modes_reply(data_field):
  #bitfield definitions according to Mode S spec
  #(start bit, num bits)
  fields = { "df": (1,5),   "vs": (6,1),   "fs": (6,3),   "cc": (7,1),
             "sl": (9,3),   "ri": (14,4),  "ac": (20,13), "dr": (9,5),
             "um": (14,6),  "id": (20,13), "ca": (6,3),   "aa": (9,24),
             "mv": (33,56), "me": (33,56), "mb": (33,56), "ke": (6,1),
             "nd": (7,4),   "md": (11,80), "ap": (33,24), "pi": (33,24),
             "lap": (88,24), "lpi": (88,24)
           }

  #fields in each packet type (DF value)
  types = { 0: ["df", "vs", "cc", "sl", "ri", "ac", "ap"],
            4: ["df", "fs", "dr", "um", "ac", "ap"],
            5: ["df", "fs", "dr", "um", "id", "ap"],
           11: ["df", "ca", "aa", "pi"],
           16: ["df", "vs", "sl", "ri", "ac", "mv", "lap"],
           17: ["df", "ca", "aa", "me", "lpi"],
           20: ["df", "fs", "dr", "um", "ac", "mb", "lap"],
           21: ["df", "fs", "dr", "um", "id", "mb", "lap"],
           24: ["df", "ke", "nd", "md", "lap"]
          }

  subfields = { "mb": mb_reply, "me": me_reply } #TODO MV

  def is_long(self):
    return self.data > (1 << 56)

  def get_numbits(self):
    return 112 if self.is_long() else 56

  def get_type(self):
    return self.get_bits(self.fields["df"])

#TODO overload getitem to handle special parity fields

#  #type MV (extended squitter type 16) subfields
#  mv_fields = { "ara": (41,14), "mte": (60,1),  "rac": (55,4), "rat": (59,1),
#                "vds": (33,8),  "vds1": (33,4), "vds2": (37,4)
#              }

class modes_parse:
  def __init__(self, mypos):
      self.my_location = mypos
      self.cpr = cpr.cpr_decoder(self.my_location)
    
  def parse0(self, data):
    fields = data.get_fields()
    altitude = decode_alt(data["ac"], True)
    return [fields["vs"], fields["cc"], fields["sl"], fields["ri"], altitude]

  def parse4(self, data):
    fields = data.get_fields()
    altitude = decode_alt(data["ac"], True)
    return [data["fs"], data["dr"], data["um"], altitude]

  def parse5(self, data):
    return [data["fs"], data["dr"], data["um"], data["id"]]

  def parse11(self, data, ecc):
    interrogator = ecc & 0x0F
    return [data["aa"], interrogator, data["ca"]]

  categories = [["NO INFO", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED"],\
                ["NO INFO", "SURFACE EMERGENCY VEHICLE", "SURFACE SERVICE VEHICLE", "FIXED OBSTRUCTION", "RESERVED", "RESERVED", "RESERVED"],\
                ["NO INFO", "GLIDER", "BALLOON/BLIMP", "PARACHUTE", "ULTRALIGHT", "RESERVED", "UAV", "SPACECRAFT"],\
                ["NO INFO", "LIGHT", "SMALL", "LARGE", "LARGE HIGH VORTEX", "HEAVY", "HIGH PERFORMANCE", "ROTORCRAFT"]]

  def parseBDS08(self, data):
    catstring = self.categories[data["me"]["ftc"]-1][data["me"]["cat"]]

    msg = ""
    for i in range(0, 8):
      msg += self.charmap( data["me"]["ident"] >> (42-6*i) & 0x3F)
    return (msg, catstring)

  def charmap(self, d):
    if d > 0 and d < 27:
      retval = chr(ord("A")+d-1)
    elif d == 32:
      retval = " "
    elif d > 47 and d < 58:
      retval = chr(ord("0")+d-48)
    else:
      retval = " "

    return retval

  def parseBDS05(self, data):
    icao24 = data["aa"]

    encoded_lon = data["me"]["lon"]
    encoded_lat = data["me"]["lat"]
    cpr_format = data["me"]["cpr"]
    altitude = decode_alt(data["me"]["alt"], False)

    [decoded_lat, decoded_lon, rnge, bearing] = self.cpr.decode(icao24, encoded_lat, encoded_lon, cpr_format, 0)

    return [altitude, decoded_lat, decoded_lon, rnge, bearing]


  #welp turns out it looks like there's only 17 bits in the BDS0,6 ground packet after all.
  def parseBDS06(self, data):
    icao24 = data["aa"]
 
    encoded_lon = data["me"]["lon"]
    encoded_lat = data["me"]["lat"]
    cpr_format = data["me"]["cpr"]
    altitude = decode_alt(data["me"]["alt"], False)
    [decoded_lat, decoded_lon, rnge, bearing] = self.cpr.decode(icao24, encoded_lat, encoded_lon, cpr_format, 1)
    return [altitude, decoded_lat, decoded_lon, rnge, bearing]

  def parseBDS09_0(self, data):
    #0: ["sub", "dew", "vew", "dns", "vns", "str", "tr", "svr", "vr"],
    bdsobj = data["me"]["bds09"]
    vert_spd = bdsobj["vr"] * 32
    ud = bool(bdsobj["dvr"])
    if ud:
      vert_spd = 0 - vert_spd
    turn_rate = bdsobj["tr"] * 15/62
    rl = bdsobj["str"]
    if rl:
      turn_rate = 0 - turn_rate
    ns_vel = bdsobj["vns"] - 1
    ns = bool(bdsobj["dns"])
    ew_vel = bdsobj["vew"] - 1
    ew = bool(bdsobj["dew"])
    
    velocity = math.hypot(ns_vel, ew_vel)
    if ew:
      ew_vel = 0 - ew_vel
    if ns:
      ns_vel = 0 - ns_vel
    heading = math.atan2(ew_vel, ns_vel) * (180.0 / math.pi)
    if heading < 0:
      heading += 360

    return [velocity, heading, vert_spd, turn_rate]

  def parseBDS09_1(self, data):
    #1: ["sub", "icf", "ifr", "nuc", "gdew", "gvew", "gdns", "gvns", "vrs", "gsvr", "gvr", "ghds", "ghd"],
    bdsobj = data["me"]["bds09"]
    alt_geo_diff = bdsobj["ghd"]
    above_below = bool(bdsobj["ghds"])
    if above_below:
      alt_geo_diff = 0 - alt_geo_diff;
    vert_spd = float(bdsobj["gvr"] - 1)
    ud = bool(bdsobj["gdvr"])
    if ud:
      vert_spd = 0 - vert_spd
    vert_src = bool((longdata >> 20) & 1)
    ns_vel = float((longdata >> 21) & 0x3FF - 1)
    ns = bool((longdata >> 31) & 1)
    ew_vel = float((longdata >> 32) & 0x3FF - 1)
    ew = bool((longdata >> 42) & 1)
    subtype = (longdata >> 48) & 0x07


    if subtype == 0x02:
      ns_vel *= 4
      ew_vel *= 4


    vert_spd *= 64
    alt_geo_diff *= 25
	
    velocity = math.hypot(ns_vel, ew_vel)
    if ew:
      ew_vel = 0 - ew_vel
	
    if ns_vel == 0:
      heading = 0
    else:
      heading = math.atan(float(ew_vel) / float(ns_vel)) * (180.0 / math.pi)
    if ns:
      heading = 180 - heading
    if heading < 0:
      heading += 360

    return [velocity, heading, vert_spd]

  def parse20(self, shortdata, longdata):
    [fs, dr, um, alt] = self.parse4(shortdata)
    #BDS defines TCAS reply type and is the first 8 bits
    #BDS1 is first four, BDS2 is bits 5-8
    bds1 = longdata_bits(longdata, 33, 4)
    bds2 = longdata_bits(longdata, 37, 4)
    #bds2 != 0 defines extended TCAS capabilities, not in spec
    return [fs, dr, um, alt, bds1, bds2]

  def parseMB_commB(self, longdata): #bds1, bds2 == 0
    raise NoHandlerError

  def parseMB_caps(self, longdata): #bds1 == 1, bds2 == 0
    #cfs, acs, bcs
    raise NoHandlerError

  def parseMB_id(self, longdata): #bds1 == 2, bds2 == 0
    msg = ""
    for i in range(0, 8):
      msg += self.charmap( longdata >> (42-6*i) & 0x3F)
    return (msg)

  def parseMB_TCASRA(self, longdata): #bds1 == 3, bds2 == 0
    #ara[41-54],rac[55-58],rat[59],mte[60],tti[61-62],tida[63-75],tidr[76-82],tidb[83-88]
    raise NoHandlerError