11 Commits

Author SHA1 Message Date
Nick Foster
1a0ebab29c Temp commit while adding a modes_report class using named tuples. Mlat server definitely broken as a result. 2013-01-09 23:26:49 -08:00
Nick Foster
7f6a3c7779 Return data works. Just need a reasonable way to get mlat data into the output plugins. 2012-12-19 18:57:23 -08:00
Nick Foster
fd12402462 mlat server working in the client-server direction. other way round untested. 2012-12-18 19:23:52 -08:00
Nick Foster
1e2b8a4f46 Split the int timestamp from frac timestamp so you don't lose precision when using, say, UTC time. Cleaned up some cruft while I was at it. This also allows devices which don't have timestamps to tag based on samples elapsed since the flowgraph started. 2012-12-12 17:40:35 -08:00
Nick Foster
3be6e9fd6e Remove altitude-based extra station. I don't now believe there's a way to construct a "fake" station as you don't have the originating time of the transmission as a known quantity. 2012-12-11 09:44:21 -08:00
Nick Foster
c4c63b5b69 Fixed it by using a reasonable initial guess to guarantee monotonicity in the solver, and to avoid converging on the wrong solution.
If all your stations lie in a plane (they do), there is an ambiguity about that plane such that there are two solutions.
This means you need to provide an initial guess on the correct side of the plane (i.e., above ground).
In addition, using a very reasonable solution guarantees that the system is as linear as possible.
I'm using the nearest station as a guess; I would have assumed it would work with any station as a guess but this
doesn't appear to be the case.
The Arne Saknussemm is still broken.
2012-12-11 09:11:35 -08:00
Nick Foster
0384d6bc58 Temp commit. mlat only resolves when the aircraft is sufficiently out of plane of the receivers -- 4000km out of plane, to be exact. What gives? 2012-12-10 18:52:04 -08:00
Nick Foster
017cce7ec4 Temp commit before I rip out the relative stuff 2012-12-07 09:36:16 -08:00
Nick Foster
1f0ef143a0 Fixed your mlat bug -- you'd removed the time error column from H and it happened to work with the extra information gathered by having timestamp[0] equal to the originating time -- i.e., zero time offset.
There's currently a bug in the solver returning near-ground-level reports for aircraft at altitude. Also the Arne Saknussemm station doesn't work.
2012-12-06 19:17:40 -08:00
Nick Foster
c7e216bca0 mlat test now creates a cheesy moving simulated aircraft. mlat is broken though due to incorrect assumptions in the solver. 2012-12-06 13:04:11 -08:00
Nick Foster
e771c21730 First stab at multilateration server. No client, incomplete. 2012-12-05 11:28:59 -08:00
42 changed files with 1669 additions and 1786 deletions

View File

@@ -1,4 +1,4 @@
# Copyright 2011,2013 Free Software Foundation, Inc. # Copyright 2011 Free Software Foundation, Inc.
# #
# This file is part of GNU Radio # This file is part of GNU Radio
# #
@@ -47,7 +47,23 @@ endif()
######################################################################## ########################################################################
# Find boost # Find boost
######################################################################## ########################################################################
include(GrBoost) if(UNIX AND EXISTS "/usr/lib64")
list(APPEND BOOST_LIBRARYDIR "/usr/lib64") #fedora 64-bit fix
endif(UNIX AND EXISTS "/usr/lib64")
set(Boost_ADDITIONAL_VERSIONS
"1.35.0" "1.35" "1.36.0" "1.36" "1.37.0" "1.37" "1.38.0" "1.38" "1.39.0" "1.39"
"1.40.0" "1.40" "1.41.0" "1.41" "1.42.0" "1.42" "1.43.0" "1.43" "1.44.0" "1.44"
"1.45.0" "1.45" "1.46.0" "1.46" "1.47.0" "1.47" "1.48.0" "1.48" "1.49.0" "1.49"
"1.50.0" "1.50" "1.51.0" "1.51" "1.52.0" "1.52" "1.53.0" "1.53" "1.54.0" "1.54"
"1.55.0" "1.55" "1.56.0" "1.56" "1.57.0" "1.57" "1.58.0" "1.58" "1.59.0" "1.59"
"1.60.0" "1.60" "1.61.0" "1.61" "1.62.0" "1.62" "1.63.0" "1.63" "1.64.0" "1.64"
"1.65.0" "1.65" "1.66.0" "1.66" "1.67.0" "1.67" "1.68.0" "1.68" "1.69.0" "1.69"
)
find_package(Boost "1.35")
if(NOT Boost_FOUND)
message(FATAL_ERROR "Boost required to compile gr-air-modes")
endif()
######################################################################## ########################################################################
# Install directories # Install directories
@@ -69,10 +85,15 @@ set(GRC_BLOCKS_DIR ${GR_PKG_DATA_DIR}/grc/blocks)
######################################################################## ########################################################################
# Find gnuradio build dependencies # Find gnuradio build dependencies
######################################################################## ########################################################################
find_package(GnuradioRuntime) find_package(Gruel)
find_package(GnuradioCore)
if(NOT GNURADIO_RUNTIME_FOUND) if(NOT GRUEL_FOUND)
message(FATAL_ERROR "GnuRadio Runtime required to compile gr-air-modes") message(FATAL_ERROR "Gruel required to compile gr-air-modes")
endif()
if(NOT GNURADIO_CORE_FOUND)
message(FATAL_ERROR "GnuRadio Core required to compile gr-air-modes")
endif() endif()
######################################################################## ########################################################################
@@ -81,12 +102,14 @@ endif()
include_directories( include_directories(
${CMAKE_SOURCE_DIR}/include ${CMAKE_SOURCE_DIR}/include
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${GNURADIO_RUNTIME_INCLUDE_DIRS} ${GRUEL_INCLUDE_DIRS}
${GNURADIO_CORE_INCLUDE_DIRS}
) )
link_directories( link_directories(
${Boost_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS}
${GNURADIO_RUNTIME_LIBRARY_DIRS} ${GRUEL_LIBRARY_DIRS}
${GNURADIO_CORE_LIBRARY_DIRS}
) )
# Set component parameters # Set component parameters

View File

@@ -21,6 +21,7 @@ include(GrPython)
GR_PYTHON_INSTALL( GR_PYTHON_INSTALL(
PROGRAMS PROGRAMS
mlat_server
modes_rx modes_rx
modes_gui modes_gui
uhd_modes.py uhd_modes.py

265
apps/mlat_server Executable file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python
#
# Copyright 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.
#
#simple multilateration server app.
#looks at received messages and then attempts to multilaterate positions
#will not attempt clock synchronization yet; it's up to clients to present
#accurate timestamps. later on we can do clock sync based on ADS-B packets.
#SECURITY NOTE: THIS SERVER WAS WRITTEN WITH NO REGARD TOWARD SECURITY.
#NO REAL ATTEMPT AT DATA VALIDATION, MUCH LESS SECURITY, HAS BEEN MADE.
#USE THIS PROGRAM AT YOUR OWN RISK AND WITH THE UNDERSTANDING THAT THE
#INTERNET IS A SCARY PLACE FULL OF MEAN PEOPLE.
import time, os, sys, socket, struct
from string import split, join
from datetime import *
import air_modes
import pickle
import time
import bisect
#change this to 0 for ASCII format for debugging. use HIGHEST_PROTOCOL
#for actual use to keep the pickle size down.
pickle_prot = 0
#pickle_prot = pickle.HIGHEST_PROTOCOL
def ordered_insert(a, item):
a.insert(bisect.bisect_right(a, item), item)
class client_info:
def __init__(self):
self.name = ""
self.position = []
self.offset_secs = 0
self.offset_frac_secs = 0.0
self.time_source = None
class connection:
def __init__(self, addr, sock, clientinfo):
self.addr = addr
self.sock = sock
self.clientinfo = clientinfo
class mlat_server:
def __init__(self, mypos, port):
self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._s.bind(('', port))
self._s.listen(1)
self._s.setblocking(0) #nonblocking
self._conns = [] #list of active connections
self._reports = {} #hashed by data
self._lastreport = 0 #used for pruning
self._parser = air_modes.parse(None)
self._remnant = None #for piecing together messages across packets
def __del__(self):
self._s.close()
def get_messages(self):
num=0
for conn in self._conns:
pkt = None
try:
pkt = conn.sock.recv(16384)
except socket.error:
print "%s disconnected" % conn.clientinfo.name
self._conns.remove(conn)
if not pkt: break
try:
for msg in pkt.splitlines(True):
if msg.endswith("\n"):
if self._remnant:
msg = self._remnant + msg
self._remnant = None
[data, ecc, reference, timestamp_int, timestamp_frac] = msg.split()
st = stamp(conn.clientinfo, int(timestamp_int), float(timestamp_frac))
if data not in self._reports:
self._reports[data] = []
#ordered insert
ordered_insert(self._reports[data], st)
if st.tofloat() > self._lastreport:
self._lastreport = st.tofloat()
num += 1
else:
if self._remnant is not None:
raise Exception("Malformed data: " + msg)
else:
self._remnant = msg
except Exception as e:
print "Invalid message from %s: %s..." % (conn.addr, pkt[:64])
print e
#self.prune()
return num
#prune should delete all reports in self._reports older than 5s.
#not really tested well
def prune(self):
for data, stamps in self._reports.iteritems():
#check the last stamp first so we don't iterate over
#the whole list if we don't have to
if self._lastreport - stamps[-1].tofloat() > 5:
del self._reports[data]
else:
for i,st in enumerate(stamps):
if self._lastreport - st.tofloat() > 5:
del self._reports[data][i]
if len(self._reports[data]) == 0:
del self._reports[data]
#return a list of eligible messages for multilateration
#eligible reports are:
#1. bit-identical
#2. from distinct stations (at least 3)
#3. within 0.003 seconds of each other
#traverse the reports for each data pkt (hash) looking for >3 reports
#within 0.003s, then check for unique IPs (this should pass 99% of the time)
#let's break a record for most nested loops. this one goes four deep.
#it's loop-ception!
def get_eligible_reports(self):
groups = []
for data,stamps in self._reports.iteritems():
if len(stamps) >= 4: #quick check before we do a set()
stations = set([st.clientinfo for st in stamps])
if len(stations) >= 4:
i=0
#it's O(n) since the list is already sorted
#can probably be cleaner and more concise
while(i < len(stamps)):
refstamp = stamps[i].tofloat()
reps = []
while (i<len(stamps)) and (stamps[i].tofloat() < (refstamp + 0.003)):
reps.append(stamps[i])
i+=1
deduped = []
for cinfo in stations:
for st in reps[::-1]:
if st.clientinfo == cinfo:
deduped.append(st)
break
if len(deduped) >= 4:
groups.append({"data": data, "stamps": deduped})
#TODO: pop the entries so you don't issue duplicate reports
if len(groups) > 0:
return groups
return None
#issue multilaterated positions
def output(self, msg):
if msg is not None:
try:
for conn in self._conns[:]: #iterate over a copy of the list
conn.sock.send(msg)
except socket.error:
print "%s disconnected" % conn.clientinfo.name
self._conns.remove(conn)
print "Connections: ", len(self._conns)
#add a new connection to the list
def add_pending_conns(self):
try:
conn, addr = self._s.accept()
conn.send("HELO") #yeah it's like that
msg = conn.recv(1024)
if not msg:
return
try:
clientinfo = pickle.loads(msg)
except:
print "Invalid pickle received from client"
return
if clientinfo.__class__.__name__ != "client_info":
print "Invalid datatype received from client"
return
self._conns.append(connection(addr[0], conn, clientinfo))
print "New connection from %s: %s" % (addr[0], clientinfo.name)
except socket.error:
pass
#retrieve altitude from a mode S packet or None if not available
#returns alt in meters
def get_modes_altitude(data):
df = data["df"] #reply type
f2m = 0.3048
if df == 0 or df == 4:
return air_modes.altitude.decode_alt(data["ac"], True)
elif df == 17:
bds = data["me"].get_type()
if bds == 0x05:
return f2m*air_modes.altitude.decode_alt(data["me"]["alt"], False)
else:
return None
position = [37.76, -117.443, 100]
if __name__=="__main__":
srv = mlat_server(position, 19005)
while 1:
nummsgs = srv.get_messages()
if len(srv._conns) > 0:
print "Received %i messages" % nummsgs
srv.add_pending_conns()
reps = srv.get_eligible_reports()
if reps:
for rep in reps:
print "Report with data %x" % rep["data"]
for st in rep["stamps"]:
print "Stamp from %s: %f" % (st.clientinfo.name, st.tofloat())
srv.prune()
#now format the reports to get them ready for multilateration
#it's expecting a list of tuples [(station[], timestamp)...]
#also have to parse the data to pull altitude out of the mix
if reps:
for rep in reps:
alt = get_modes_altitude(air_modes.modes_reply(rep["data"]))
if (alt is None and len(rep["stamps"]) > 3) or alt is not None:
mlat_list = [(st.clientinfo.position, st.tofloat()) for st in rep["stamps"]]
print mlat_list
#multilaterate!
try:
pos, senttime = air_modes.mlat.mlat(mlat_list, alt)
if pos is not None:
print "Resolved position: ", pos
#send a report!
msg = "%x %i %i %f %f %f %f %f %f\n" % \
(rep["data"], len(mlat_list), int(senttime), \
float(senttime) - int(senttime), \
pos[0], pos[1], pos[2], 0.0, 0.0)
srv.output(msg)
except air_modes.exceptions.MlatNonConvergeError as e:
print e
#DEBUG
# else:
# msg = "0921354 4 2 0.12345 36.671234 -112.342515 9028.1243 0.0 0.0\n"
# srv.output(msg)
time.sleep(0.3)

67
apps/mlat_test.py Executable file
View File

@@ -0,0 +1,67 @@
#!/usr/bin/env python
#test stuff for mlat server
import socket, pickle, time, random, sys, math, numpy
import air_modes
class rx_data:
secs = 0
frac_secs = 0.0
data = None
class client_info:
def __init__(self):
self.name = ""
self.position = []
self.offset_secs = 0
self.offset_frac_secs = 0.0
info = client_info()
info.name = sys.argv[1]
info.position = [float(sys.argv[2]), float(sys.argv[3]), 100]
info.offset_secs = 0
info.offset_frac_secs = 0.0
data1 = rx_data()
data1.secs = 0
data1.data = int("0x8da81f875857f10eb65b10cb66f3", 16)
ac_starting_pos = [37.617175, -122.400843, 8000]
ac_hdg = 130.
ac_spd = 0.00008
def get_pos(time):
return [ac_starting_pos[0] + ac_spd * time * math.cos(ac_hdg*math.pi/180.), \
ac_starting_pos[1] + ac_spd * time * math.sin(ac_hdg*math.pi/180.), \
ac_starting_pos[2]]
def get_simulated_timestamp(time, position):
prange = numpy.linalg.norm(numpy.array(air_modes.mlat.llh2ecef(position))-numpy.array(air_modes.mlat.llh2geoid(info.position))) / air_modes.mlat.c
return time + prange + random.random()*60e-9 - 30e-9
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.setblocking(1)
sock.connect(("localhost", 19005))
sock.send(pickle.dumps(info))
print sock.recv(1024)
sock.setblocking(0)
ts = 0.0
while 1:
pos = get_pos(ts)
stamp = get_simulated_timestamp(ts, pos)
print "Timestamp: %.10f" % (stamp)
print "Position: ", pos
data1.secs = int(stamp)
data1.frac_secs = float(stamp)
data1.frac_secs -= int(data1.frac_secs)
sock.send(pickle.dumps([data1]))
try:
positions = sock.recv(1024)
except:
pass
if positions: print positions
ts+=1
time.sleep(1)
sock.close()
sock = None

View File

@@ -19,19 +19,16 @@
# Boston, MA 02110-1301, USA. # Boston, MA 02110-1301, USA.
# #
import os, sys, time, threading, datetime, math, csv, tempfile import os, sys, time, threading, datetime, math, csv
from optparse import OptionParser from PyQt4 import QtCore,QtGui
from PyQt4 import QtCore,QtGui,QtWebKit
from PyQt4.Qwt5 import Qwt from PyQt4.Qwt5 import Qwt
from gnuradio import gr, eng_notation from gnuradio import gr, gru, optfir, eng_notation, blks2
from gnuradio.eng_option import eng_option import gnuradio.gr.gr_threading as _threading
from gnuradio.gr.pubsub import pubsub
import air_modes import air_modes
from air_modes.exceptions import * from air_modes.exceptions import *
from air_modes.modes_rx_ui import Ui_MainWindow from air_modes.modes_rx_ui import Ui_MainWindow
from air_modes.gui_model import * from air_modes.gui_model import *
import sqlite3 import sqlite3
import zmq
class mainwindow(QtGui.QMainWindow): class mainwindow(QtGui.QMainWindow):
live_data_changed_signal = QtCore.pyqtSignal(QtCore.QString, name='liveDataChanged') live_data_changed_signal = QtCore.pyqtSignal(QtCore.QString, name='liveDataChanged')
@@ -42,7 +39,7 @@ class mainwindow(QtGui.QMainWindow):
#set defaults #set defaults
#add file, RTL, UHD sources #add file, RTL, UHD sources
self.ui.combo_source.addItems(["UHD", "Osmocom", "File/UDP"]) self.ui.combo_source.addItems(["UHD device", "RTL-SDR", "File"])
self.ui.combo_source.setCurrentIndex(0) self.ui.combo_source.setCurrentIndex(0)
#populate antenna, rate combo boxes based on source #populate antenna, rate combo boxes based on source
@@ -54,8 +51,8 @@ class mainwindow(QtGui.QMainWindow):
#default to 5dB #default to 5dB
self.ui.line_threshold.insert("5") self.ui.line_threshold.insert("5")
self.ui.prog_rssi.setMinimum(0) self.ui.prog_rssi.setMinimum(-40)
self.ui.prog_rssi.setMaximum(40) self.ui.prog_rssi.setMaximum(0)
self.ui.combo_ant.setCurrentIndex(self.ui.combo_ant.findText("RX2")) self.ui.combo_ant.setCurrentIndex(self.ui.combo_ant.findText("RX2"))
@@ -73,16 +70,18 @@ class mainwindow(QtGui.QMainWindow):
#disable by default #disable by default
self.ui.check_adsbonly.setCheckState(QtCore.Qt.Unchecked) self.ui.check_adsbonly.setCheckState(QtCore.Qt.Unchecked)
#set up the radio stuff
self.queue = gr.msg_queue(10) self.queue = gr.msg_queue(10)
self.running = False self.runner = None
self.fg = None
self.outputs = []
self.updates = []
self.output_handler = None
self.kmlgen = None #necessary bc we stop its thread in shutdown self.kmlgen = None #necessary bc we stop its thread in shutdown
self.dbname = "adsb.db" self.dbname = "air_modes.db"
self.num_reports = 0 self.num_reports = 0
self.last_report = 0 self.last_report = 0
self.context = zmq.Context(1)
self.datamodel = dashboard_sql_model(None) self.datamodel = dashboard_data_model(None)
self.ui.list_aircraft.setModel(self.datamodel) self.ui.list_aircraft.setModel(self.datamodel)
self.ui.list_aircraft.setModelColumn(0) self.ui.list_aircraft.setModelColumn(0)
@@ -96,16 +95,15 @@ class mainwindow(QtGui.QMainWindow):
self.dashboard_mapper.setModel(self.datamodel) self.dashboard_mapper.setModel(self.datamodel)
self.dashboard_mapper.addMapping(self.ui.line_icao, 0) self.dashboard_mapper.addMapping(self.ui.line_icao, 0)
#self.dashboard_mapper.addMapping(self.ui.prog_rssi, 2) #self.dashboard_mapper.addMapping(self.ui.prog_rssi, 2)
self.dashboard_mapper.addMapping(self.ui.line_latitude, 2) self.dashboard_mapper.addMapping(self.ui.line_latitude, 3)
self.dashboard_mapper.addMapping(self.ui.line_longitude, 3) self.dashboard_mapper.addMapping(self.ui.line_longitude, 4)
self.dashboard_mapper.addMapping(self.ui.line_alt, 4) self.dashboard_mapper.addMapping(self.ui.line_alt, 5)
self.dashboard_mapper.addMapping(self.ui.line_speed, 5) self.dashboard_mapper.addMapping(self.ui.line_speed, 6)
#self.dashboard_mapper.addMapping(self.ui.compass_heading, 6) #self.dashboard_mapper.addMapping(self.ui.compass_heading, 7)
self.dashboard_mapper.addMapping(self.ui.line_climb, 7) self.dashboard_mapper.addMapping(self.ui.line_climb, 8)
self.dashboard_mapper.addMapping(self.ui.line_ident, 8) self.dashboard_mapper.addMapping(self.ui.line_ident, 9)
self.dashboard_mapper.addMapping(self.ui.line_type, 9) self.dashboard_mapper.addMapping(self.ui.line_type, 10)
#self.dashboard_mapper.addMapping(self.ui.line_range, 11) self.dashboard_mapper.addMapping(self.ui.line_range, 11)
#self.dashboard_mapper.addMapping(self.ui.compass_bearing, 12)
compass_palette = QtGui.QPalette() compass_palette = QtGui.QPalette()
compass_palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.white) compass_palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.white)
@@ -115,14 +113,12 @@ class mainwindow(QtGui.QMainWindow):
self.ui.compass_heading.setNeedle(Qwt.QwtDialSimpleNeedle(Qwt.QwtDialSimpleNeedle.Ray, False, QtCore.Qt.black)) self.ui.compass_heading.setNeedle(Qwt.QwtDialSimpleNeedle(Qwt.QwtDialSimpleNeedle.Ray, False, QtCore.Qt.black))
self.ui.compass_bearing.setNeedle(Qwt.QwtDialSimpleNeedle(Qwt.QwtDialSimpleNeedle.Ray, False, QtCore.Qt.black)) self.ui.compass_bearing.setNeedle(Qwt.QwtDialSimpleNeedle(Qwt.QwtDialSimpleNeedle.Ray, False, QtCore.Qt.black))
#hook up the update signals which are explicitly necessary #hook up the update signal
#most of the dashboard_mapper and list_aircraft stuff is implicitly done already
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.dashboard_mapper.setCurrentModelIndex) self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.dashboard_mapper.setCurrentModelIndex)
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_heading_widget) self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_heading_widget)
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_bearing_widget) self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_bearing_widget)
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_rssi_widget)
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_map_highlight)
self.datamodel.dataChanged.connect(self.unmapped_widgets_dataChanged) self.datamodel.dataChanged.connect(self.unmapped_widgets_dataChanged)
self.ui.list_aircraft.selectionModel().currentRowChanged.connect(self.update_rssi_widget)
#hook up live data text box update signal #hook up live data text box update signal
self.live_data_changed_signal.connect(self.on_append_live_data) self.live_data_changed_signal.connect(self.on_append_live_data)
@@ -130,29 +126,29 @@ class mainwindow(QtGui.QMainWindow):
############ widget update functions for non-mapped widgets ############ ############ widget update functions for non-mapped widgets ############
def update_heading_widget(self, index): def update_heading_widget(self, index):
if index.model() is not None: if index.model() is not None:
heading = index.model().data(index.model().index(index.row(), 6)).toDouble()[0] heading = index.model().data(index.model().index(index.row(), self.datamodel._colnames.index("heading"))).toDouble()[0]
self.ui.compass_heading.setValue(heading) self.ui.compass_heading.setValue(heading)
def update_bearing_widget(self, index): def update_bearing_widget(self, index):
if index.model() is not None: if index.model() is not None:
bearing = 0#index.model().data(index.model().index(index.row(), 12)).toDouble()[0] bearing = index.model().data(index.model().index(index.row(), self.datamodel._colnames.index("bearing"))).toDouble()[0]
self.ui.compass_bearing.setValue(bearing) self.ui.compass_bearing.setValue(bearing)
def unmapped_widgets_dataChanged(self, startIndex, endIndex): def unmapped_widgets_dataChanged(self, startIndex, endIndex):
index = self.ui.list_aircraft.selectionModel().currentIndex() index = self.ui.list_aircraft.selectionModel().currentIndex()
if index.row() in range(startIndex.row(), endIndex.row()+1): #the current aircraft was affected if index.row() in range(startIndex.row(), endIndex.row()+1): #the current aircraft was affected
if 6 in range(startIndex.column(), endIndex.column()+1): if self.datamodel._colnames.index("heading") in range(startIndex.column(), endIndex.column()+1):
self.update_heading_widget(index) self.update_heading_widget(index)
if 12 in range(startIndex.column(), endIndex.column()+1): if self.datamodel._colnames.index("bearing") in range(startIndex.column(), endIndex.column()+1):
self.update_bearing_widget(index) self.update_bearing_widget(index)
if 2 in range(startIndex.column(), endIndex.column()+1): if self.datamodel._colnames.index("rssi") in range(startIndex.column(), endIndex.column()+1):
self.update_rssi_widget(index) self.update_rssi_widget(index)
def update_rssi_widget(self, index): def update_rssi_widget(self, index):
if index.model() is not None: if index.model() is not None:
rssi = 0#index.model().data(index.model().index(index.row(), 2)).toDouble()[0] rssi = index.model().data(index.model().index(index.row(), 2)).toDouble()[0]
self.ui.prog_rssi.setValue(rssi) self.ui.prog_rssi.setValue(rssi)
def increment_reportspersec(self, msg): def increment_reportspersec(self, msg):
self.num_reports += 1 self.num_reports += 1
@@ -163,12 +159,6 @@ class mainwindow(QtGui.QMainWindow):
self.ui.line_reports.setText("%i" % self.num_reports) self.ui.line_reports.setText("%i" % self.num_reports)
self.num_reports = 0 self.num_reports = 0
def update_map_highlight(self, index):
if index.model() is not None:
icaostr = index.model().data(index.model().index(index.row(), self.datamodel._colnames.index("icao"))).toString()
icao = int(str(icaostr), 16)
self.jsonpgen.set_highlight(icao)
##################### dynamic option population ######################## ##################### dynamic option population ########################
#goes and gets valid antenna, sample rate options from the device and grays out appropriate things #goes and gets valid antenna, sample rate options from the device and grays out appropriate things
def populate_source_options(self): def populate_source_options(self):
@@ -176,13 +166,12 @@ class mainwindow(QtGui.QMainWindow):
self.rates = [] self.rates = []
self.ratetext = [] self.ratetext = []
self.antennas = [] self.antennas = []
if sourceid == "UHD": if sourceid == "UHD device":
try: try:
from gnuradio import uhd from gnuradio import uhd
self.src = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1) self.src = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1)
self.rates = [rate.start() for rate in self.src.get_samp_rates() self.rates = [rate.start() for rate in self.src.get_samp_rates()]
if (rate.start() % 2.e6) == 0 and rate >= 4e6]
self.antennas = self.src.get_antennas() self.antennas = self.src.get_antennas()
self.src = None #deconstruct UHD source for now self.src = None #deconstruct UHD source for now
self.ui.combo_ant.setEnabled(True) self.ui.combo_ant.setEnabled(True)
@@ -194,26 +183,16 @@ class mainwindow(QtGui.QMainWindow):
self.ui.combo_ant.setEnabled(False) self.ui.combo_ant.setEnabled(False)
self.ui.combo_rate.setEnabled(False) self.ui.combo_rate.setEnabled(False)
self.ui.stack_source.setCurrentIndex(0) self.ui.stack_source.setCurrentIndex(0)
elif sourceid == "RTL-SDR":
self.rates = [3.2e6]
self.antennas = ["RX"]
self.ui.combo_ant.setEnabled(False)
self.ui.combo_rate.setEnabled(False)
self.ui.stack_source.setCurrentIndex(0)
elif sourceid == "Osmocom": elif sourceid == "File":
try: self.rates = [2e6, 4e6, 6e6, 8e6, 10e6]
import osmosdr
self.src = osmosdr.source_c("")
self.rates = [rate.start() for rate in self.src.get_sample_rates() if (rate.start() % 2.e6) == 0]
self.antennas = ["RX"]
self.src = None
self.ui.combo_ant.setEnabled(False)
self.ui.combo_rate.setEnabled(True)
self.ui.stack_source.setCurrentIndex(0)
except:
self.rates = []
self.antennas = []
self.ui.combo_ant.setEnabled(False)
self.ui.combo_rate.setEnabled(False)
self.ui.stack_source.setCurrentIndex(0)
elif sourceid == "File/UDP":
self.rates = [2e6*i for i in range(2,13)]
self.antennas = ["None"] self.antennas = ["None"]
self.ui.combo_ant.setEnabled(False) self.ui.combo_ant.setEnabled(False)
self.ui.combo_rate.setEnabled(True) self.ui.combo_rate.setEnabled(True)
@@ -227,12 +206,8 @@ class mainwindow(QtGui.QMainWindow):
self.ui.combo_ant.clear() self.ui.combo_ant.clear()
self.ui.combo_ant.addItems(self.antennas) self.ui.combo_ant.addItems(self.antennas)
#set up recommended sample rate if 4e6 in self.rates:
recommended_rate = min(x for x in self.rates if x >= 4e6 and self.ui.combo_rate.setCurrentIndex(self.rates.index(4e6))
max(self.rates) % x == 0)
if recommended_rate >= 8.e6:
self.ui.check_pmf.setChecked(True)
self.ui.combo_rate.setCurrentIndex(self.rates.index(recommended_rate))
################ action handlers #################### ################ action handlers ####################
def on_combo_source_currentIndexChanged(self, index): def on_combo_source_currentIndexChanged(self, index):
@@ -240,47 +215,47 @@ class mainwindow(QtGui.QMainWindow):
def on_button_start_released(self): def on_button_start_released(self):
#if we're already running, kill it! #if we're already running, kill it!
if self.running is True: if self.runner is not None:
self.on_quit() self.output_handler.done = True
self.output_handler = None
self.outputs = []
self.updates = []
self.fg.stop()
self.runner = None
self.fg = None
if self.kmlgen is not None:
self.kmlgen.done = True
#TODO FIXME need a way to kill kmlgen safely without delay
#self.kmlgen.join()
#self.kmlgen = None
self.num_reports = 0 self.num_reports = 0
self.ui.line_reports.setText("0") self.ui.line_reports.setText("0")
self.ui.button_start.setText("Start") self.ui.button_start.setText("Start")
self.running = False
else: #we aren't already running, let's get this party started else: #we aren't already running, let's get this party started
parser = OptionParser(option_class=eng_option) options = {}
air_modes.modes_radio.add_radio_options(parser) options["source"] = str(self.ui.combo_source.currentText())
(options, args) = parser.parse_args() #sets defaults nicely options["rate"] = float(self.ui.combo_rate.currentText()) * 1e6
if str(self.ui.combo_source.currentText()) != "File/UDP": options["antenna"] = str(self.ui.combo_ant.currentText())
options.source = str(self.ui.combo_source.currentText()).lower() options["gain"] = float(self.ui.line_gain.text())
else: options["threshold"] = float(self.ui.line_threshold.text())
options.source = str(self.ui.line_inputfile.text()) options["filename"] = str(self.ui.line_inputfile.text())
options.rate = float(self.ui.combo_rate.currentText()) * 1e6 options["pmf"] = self.ui.check_pmf.checkState()
options.antenna = str(self.ui.combo_ant.currentText())
options.gain = float(self.ui.line_gain.text())
options.threshold = float(self.ui.line_threshold.text())
options.pmf = self.ui.check_pmf.checkState()
self._servers = ["inproc://modes-radio-pub"] #TODO ADD REMOTES self.fg = adsb_rx_block(options, self.queue) #create top RX block
self._relay = air_modes.zmq_pubsub_iface(self.context, subaddr=self._servers, pubaddr=None) self.runner = top_block_runner(self.fg) #spawn new thread to do RX
if self.ui.check_raw.checkState():
options.tcp = int(self.ui.line_rawport.text())
self._radio = air_modes.modes_radio(options, self.context)
self._publisher = pubsub()
self._relay.subscribe("dl_data", air_modes.make_parser(self._publisher))
try: try:
my_position = [float(self.ui.line_my_lat.text()), float(self.ui.line_my_lon.text())] my_position = [float(self.ui.line_my_lat.text()), float(self.ui.line_my_lon.text())]
except: except:
my_position = None my_position = None
self._cpr_dec = air_modes.cpr_decoder(my_position) self.datamodelout = dashboard_output(my_position, self.datamodel)
# self.datamodelout = dashboard_output(self._cpr_dec, self.datamodel, self._publisher)
self.outputs = [self.datamodelout.output]
self.updates = []
self.lock = threading.Lock() #grab a lock to ensure sql and kml don't step on each other self.lock = threading.Lock() #grab a lock to ensure sql and kml don't step on each other
#output options to populate outputs, updates #output options to populate outputs, updates
@@ -290,61 +265,55 @@ class mainwindow(QtGui.QMainWindow):
if self.ui.check_sbs1.checkState(): if self.ui.check_sbs1.checkState():
sbs1port = int(self.ui.line_sbs1port.text()) sbs1port = int(self.ui.line_sbs1port.text())
sbs1out = air_modes.output_sbs1(self._cpr_dec, sbs1port, self._publisher) sbs1out = air_modes.output_sbs1(my_position, sbs1port)
self.outputs.append(sbs1out.output)
self.updates.append(sbs1out.add_pending_conns)
if self.ui.check_fgfs.checkState(): if self.ui.check_fgfs.checkState():
fghost = "127.0.0.1" #TODO FIXME fghost = "127.0.0.1" #TODO FIXME
fgport = self.ui.line_fgfsport.text() fgport = self.ui.line_fgfsport.text()
fgout = air_modes.output_flightgear(self._cpr_dec, fghost, int(fgport), self._publisher) fgout = air_modes.output_flightgear(my_position, fghost, int(fgport))
self.outputs.append(fgout.output)
if self.ui.check_raw.checkState():
rawport = air_modes.raw_server(int(self.ui.line_rawport.text()))
self.outputs.append(rawport.output)
self.updates.append(rawport.add_pending_conns)
#add azimuth map output and hook it up #add azimuth map output and hook it up
if my_position is not None: if my_position is not None:
self.az_map_output = air_modes.az_map_output(self._cpr_dec, self.az_model, self._publisher) self.az_map_output = air_modes.az_map_output(my_position, self.az_model)
#self._relay.subscribe("dl_data", self.az_map_output.output) self.outputs.append(self.az_map_output.output)
#set up map self.livedata = air_modes.output_print(my_position)
#NOTE this is busted on windows. WebKit requires .htm[l] extensions to render, #add output for live data box
#so using a temp file doesn't work. self.outputs.append(self.output_live_data)
self._htmlfile = open("/tmp/mode_s.html", 'wb+')#tempfile.NamedTemporaryFile()
self._jsonfile = tempfile.NamedTemporaryFile()
self.livedata = air_modes.output_print(self._cpr_dec,
self._publisher,
self.live_data_changed_signal.emit)
#create SQL database for KML and dashboard displays #create SQL database for KML and dashboard displays
self.dbwriter = air_modes.output_sql(self._cpr_dec, self.dbname, self.lock, self._publisher) self.dbwriter = air_modes.output_sql(my_position, self.dbname, self.lock)
self.jsonpgen = air_modes.output_jsonp(self._jsonfile.name, self.dbname, my_position, self.lock, timeout=1) self.outputs.append(self.dbwriter.output) #now the db will update itself
htmlstring = air_modes.html_template(my_position, self._jsonfile.name)
self._htmlfile.write(htmlstring)
self._htmlfile.flush()
class WebPage(QtWebKit.QWebPage):
def javaScriptConsoleMessage(self, msg, line, source):
print '%s line %d: %s' % (source, line, msg)
page = WebPage()
self.ui.mapView.setPage(page)
self.ui.mapView.load( QtCore.QUrl( QtCore.QUrl.fromLocalFile("/tmp/mode_s.html") ) )
self.ui.mapView.show()
#output to update reports/sec widget #output to update reports/sec widget
self._relay.subscribe("dl_data", self.increment_reportspersec) self.outputs.append(self.increment_reportspersec)
self._rps_timer = QtCore.QTimer() self.updates.append(self.update_reportspersec)
self._rps_timer.timeout.connect(self.update_reportspersec)
self._rps_timer.start(1000)
#start the flowgraph #create output handler thread
self._radio.start() self.output_handler = output_handler(self.outputs, self.updates, self.queue)
self.ui.button_start.setText("Stop") self.ui.button_start.setText("Stop")
self.running = True
def on_quit(self): def on_quit(self):
if self.running is True: if self.runner is not None:
self._relay.close() try:
self._radio.close() self.output_handler.done = True
self._relay = None except:
self._radio = None pass
self._rps_timer = None self.output_handler = None
self.outputs = []
self.updates = []
self.fg.stop()
self.runner = None
self.fg = None
try: try:
self.kmlgen.done = True self.kmlgen.done = True
#TODO FIXME need a way to kill kmlgen safely without delay #TODO FIXME need a way to kill kmlgen safely without delay
@@ -362,10 +331,109 @@ class mainwindow(QtGui.QMainWindow):
cursor.movePosition(QtGui.QTextCursor.Start) cursor.movePosition(QtGui.QTextCursor.Start)
cursor.select(QtGui.QTextCursor.LineUnderCursor) cursor.select(QtGui.QTextCursor.LineUnderCursor)
cursor.removeSelectedText() cursor.removeSelectedText()
self.ui.text_livedata.append(msgstr) self.ui.text_livedata.append(msgstr)
self.ui.text_livedata.verticalScrollBar().setSliderPosition(self.ui.text_livedata.verticalScrollBar().maximum()) self.ui.text_livedata.verticalScrollBar().setSliderPosition(self.ui.text_livedata.verticalScrollBar().maximum())
def output_live_data(self, msg):
msgstr = self.livedata.parse(msg)
if msgstr is not None:
self.live_data_changed_signal.emit(msgstr)
#the output handler is a thread which runs the various registered output functions when there's a received message.
#it also executes registered updates at the sleep rate -- currently 10Hz.
class output_handler(threading.Thread):
def __init__(self, outputs, updates, queue):
threading.Thread.__init__(self)
self.setDaemon(1)
self.outputs = outputs
self.updates = updates
self.queue = queue
self.done = False
self.start()
def run(self):
while self.done is False:
for update in self.updates:
update()
while not self.queue.empty_p():
msg = self.queue.delete_head()
for output in self.outputs:
try:
output(msg.to_string())
except ADSBError:
pass
time.sleep(0.1)
self.done = True
self.outputs = None
self.updates = None
self.queue = None
class top_block_runner(_threading.Thread):
def __init__(self, tb):
_threading.Thread.__init__(self)
self.setDaemon(1)
self.tb = tb
self.done = False
self.start()
def run(self):
self.tb.run()
self.done = True
#Top block for ADSB receiver. If you define a standard interface you
#can make this common code between the GUI app and the cmdline app
class adsb_rx_block (gr.top_block):
def __init__(self, options, queue):
gr.top_block.__init__(self)
self.options = options
rate = options["rate"]
print "Rate: %f" % rate
use_resampler = False
freq = 1090e6
if options["source"] == "UHD device":
from gnuradio import uhd
self.u = uhd.single_usrp_source("", uhd.io_type_t.COMPLEX_FLOAT32, 1)
time_spec = uhd.time_spec(0.0)
self.u.set_time_now(time_spec)
self.u.set_antenna(options["antenna"])
self.u.set_samp_rate(rate)
rate = self.u.get_samp_rate()
self.u.set_gain(int(options["gain"]))
self.u.set_center_freq(freq, 0)
elif options["source"] == "RTL-SDR":
import osmosdr
self.u = osmosdr.source_c()
self.u.set_sample_rate(3.2e6) #fixed for RTL dongles
rate = int(4e6)
self.u.set_gain_mode(0) #manual gain mode
self.u.set_gain(int(options["gain"]))
self.u.set_center_freq(freq, 0)
use_resampler = True
elif options["source"] == "File":
self.u = gr.file_source(gr.sizeof_gr_complex, options["filename"])
else:
raise NotImplementedError
self.rx_path = air_modes.rx_path(rate, options["threshold"], queue, options["pmf"])
if use_resampler:
self.lpfiltcoeffs = gr.firdes.low_pass(1, 5*3.2e6, 1.6e6, 300e3)
self.resample = blks2.rational_resampler_ccf(interpolation=5, decimation=4, taps=self.lpfiltcoeffs)
self.connect(self.u, self.resample, self.rx_path)
else:
self.connect(self.u, self.rx_path)
if __name__ == '__main__': if __name__ == '__main__':
app = QtGui.QApplication(sys.argv) app = QtGui.QApplication(sys.argv)
window = mainwindow() window = mainwindow()

View File

@@ -1,5 +1,5 @@
#!/usr/bin/env python #!/usr/bin/env python
# Copyright 2010, 2013 Nick Foster # Copyright 2010 Nick Foster
# #
# This file is part of gr-air-modes # This file is part of gr-air-modes
# #
@@ -19,80 +19,256 @@
# Boston, MA 02110-1301, USA. # Boston, MA 02110-1301, USA.
# #
my_position = None
from gnuradio import gr, gru, optfir, eng_notation, blks2
from gnuradio.eng_option import eng_option from gnuradio.eng_option import eng_option
from gnuradio.gr.pubsub import pubsub
from optparse import OptionParser from optparse import OptionParser
import time, os, sys, threading, math import time, os, sys, threading
from string import split, join from string import split, join
import air_modes import air_modes
from air_modes.types import * from air_modes.types import modes_report, stamp
from air_modes.parse import modes_reply
import gnuradio.gr.gr_threading as _threading
import csv
from air_modes.exceptions import * from air_modes.exceptions import *
import zmq import pickle
#todo: maybe move plugins to separate programs (flightgear, SBS1, etc.) class top_block_runner(_threading.Thread):
def main(): def __init__(self, tb):
my_position = None _threading.Thread.__init__(self)
usage = "%prog: [options]" self.setDaemon(1)
optparser = OptionParser(option_class=eng_option, usage=usage) self.tb = tb
air_modes.modes_radio.add_radio_options(optparser) self.done = False
self.start()
optparser.add_option("-l","--location", type="string", default=None, def run(self):
help="GPS coordinates of receiving station in format xx.xxxxx,xx.xxxxx") self.tb.run()
#data source options self.done = True
optparser.add_option("-a","--remote", type="string", default=None,
help="specify additional servers from which to take data in format tcp://x.x.x.x:y,tcp://....")
optparser.add_option("-n","--no-print", action="store_true", default=False,
help="disable printing decoded packets to stdout")
#output plugins
optparser.add_option("-K","--kml", type="string", default=None,
help="filename for Google Earth KML output")
optparser.add_option("-P","--sbs1", action="store_true", default=False,
help="open an SBS-1-compatible server on port 30003")
optparser.add_option("-m","--multiplayer", type="string", default=None,
help="FlightGear server to send aircraft data, in format host:port")
(options, args) = optparser.parse_args() class adsb_rx_block (gr.top_block):
def __init__(self, options, args, queue):
gr.top_block.__init__(self)
#construct the radio self.options = options
context = zmq.Context(1) self.args = args
tb = air_modes.modes_radio(options, context) rate = int(options.rate)
servers = ["inproc://modes-radio-pub"] use_resampler = False
if options.remote is not None: self.time_source = None
servers += options.remote.split(",")
relay = air_modes.zmq_pubsub_iface(context, subaddr=servers, pubaddr=None) if options.filename is None and options.udp is None and not options.rtlsdr:
publisher = pubsub() #UHD source by default
relay.subscribe("dl_data", air_modes.make_parser(publisher)) from gnuradio import uhd
self.u = uhd.single_usrp_source(options.args, uhd.io_type_t.COMPLEX_FLOAT32, 1)
#check for GPSDO
#if you have a GPSDO, UHD will automatically set the timestamp to UTC time
#as well as automatically set the clock to lock to GPSDO.
if self.u.get_time_source(0) == 'gpsdo':
self.time_source = 'gpsdo'
else:
self.time_source = None
self.u.set_time_now(uhd.time_spec(0.0))
if not options.antenna is None:
self.u.set_antenna(options.antenna)
self.u.set_samp_rate(rate)
rate = int(self.u.get_samp_rate()) #retrieve actual
if options.gain is None: #set to halfway
g = self.u.get_gain_range()
options.gain = (g.start()+g.stop()) / 2.0
if not(self.tune(options.freq)):
print "Failed to set initial frequency"
print "Setting gain to %i" % options.gain
self.u.set_gain(options.gain)
print "Gain is %i" % self.u.get_gain()
elif options.rtlsdr: #RTLSDR dongle
import osmosdr
self.u = osmosdr.source_c(options.args)
self.u.set_sample_rate(3.2e6) #fixed for RTL dongles
if not self.u.set_center_freq(options.freq):
print "Failed to set initial frequency"
self.u.set_gain_mode(0) #manual gain mode
if options.gain is None:
options.gain = 34
self.u.set_gain(options.gain)
print "Gain is %i" % self.u.get_gain()
use_resampler = True
else:
if options.filename is not None:
self.u = gr.file_source(gr.sizeof_gr_complex, options.filename)
elif options.udp is not None:
self.u = gr.udp_source(gr.sizeof_gr_complex, "localhost", options.udp)
else:
raise Exception("No valid source selected")
print "Rate is %i" % (rate,)
pass_all = 0
if options.output_all :
pass_all = 1
self.rx_path = air_modes.rx_path(rate, options.threshold, queue, options.pmf)
if use_resampler:
self.lpfiltcoeffs = gr.firdes.low_pass(1, 5*3.2e6, 1.6e6, 300e3)
self.resample = blks2.rational_resampler_ccf(interpolation=5, decimation=4, taps=self.lpfiltcoeffs)
self.connect(self.u, self.resample, self.rx_path)
else:
self.connect(self.u, self.rx_path)
def tune(self, freq):
result = self.u.set_center_freq(freq, 0)
return result
def printraw(msg):
print msg
def printmlat(msg):
print "Mlat report: %s" % msg
if __name__ == '__main__':
usage = "%prog: [options] output filename"
parser = OptionParser(option_class=eng_option, usage=usage)
parser.add_option("-R", "--rx-subdev-spec", type="string",
help="select USRP Rx side A or B", metavar="SUBDEV")
parser.add_option("-A", "--antenna", type="string",
help="select which antenna to use on daughterboard")
parser.add_option("-D", "--args", type="string",
help="arguments to pass to UHD/RTL constructor", default="")
parser.add_option("-f", "--freq", type="eng_float", default=1090e6,
help="set receive frequency in Hz [default=%default]", metavar="FREQ")
parser.add_option("-g", "--gain", type="int", default=None,
help="set RF gain", metavar="dB")
parser.add_option("-r", "--rate", type="eng_float", default=4000000,
help="set ADC sample rate [default=%default]")
parser.add_option("-T", "--threshold", type="eng_float", default=5.0,
help="set pulse detection threshold above noise in dB [default=%default]")
parser.add_option("-a","--output-all", action="store_true", default=False,
help="output all frames")
parser.add_option("-F","--filename", type="string", default=None,
help="read data from file instead of USRP")
parser.add_option("-K","--kml", type="string", default=None,
help="filename for Google Earth KML output")
parser.add_option("-P","--sbs1", action="store_true", default=False,
help="open an SBS-1-compatible server on port 30003")
parser.add_option("-w","--raw", action="store_true", default=False,
help="open a server outputting raw timestamped data on port 9988")
parser.add_option("-n","--no-print", action="store_true", default=False,
help="disable printing decoded packets to stdout")
parser.add_option("-l","--location", type="string", default=None,
help="GPS coordinates of receiving station in format xx.xxxxx,xx.xxxxx")
parser.add_option("-u","--udp", type="int", default=None,
help="Use UDP source on specified port")
parser.add_option("-m","--multiplayer", type="string", default=None,
help="FlightGear server to send aircraft data, in format host:port")
parser.add_option("-d","--rtlsdr", action="store_true", default=False,
help="Use RTLSDR dongle instead of UHD source")
parser.add_option("-p","--pmf", action="store_true", default=False,
help="Use pulse matched filtering")
parser.add_option("-s","--mlat-server", type="string", default=None,
help="Use a multilateration server to track non-ADS-B aircraft")
(options, args) = parser.parse_args()
if options.location is not None: if options.location is not None:
my_position = [float(n) for n in options.location.split(",")] reader = csv.reader([options.location], quoting=csv.QUOTE_NONNUMERIC)
my_position = reader.next()
#CPR decoder obj to handle getting position from BDS0,5 and BDS0,6 pkts queue = gr.msg_queue()
cpr_dec = air_modes.cpr_decoder(my_position) mlat_queue = None
outputs = [] #registry of plugin output functions
mlat_outputs = [] #registry of plugin mlat handling functions
updates = [] #registry of plugin update functions
if options.raw is True:
rawport = air_modes.raw_server(9988) #port
outputs.append(rawport.output)
outputs.append(printraw)
updates.append(rawport.add_pending_conns)
if options.kml is not None: if options.kml is not None:
#we spawn a thread to run every 30 seconds (or whatever) to generate KML
dbname = 'adsb.db' dbname = 'adsb.db'
lock = threading.Lock() lock = threading.Lock()
sqldb = air_modes.output_sql(cpr_dec, dbname, lock, publisher) #input into the db sqldb = air_modes.output_sql(my_position, dbname, lock) #input into the db
kmlgen = air_modes.output_kml(options.kml, dbname, my_position, lock) #create a KML generating thread to read from the db kmlgen = air_modes.output_kml(options.kml, dbname, my_position, lock) #create a KML generating thread to read from the db
outputs.append(sqldb.output)
if options.sbs1 is True:
sbs1port = air_modes.output_sbs1(my_position, 30003)
outputs.append(sbs1port.output)
updates.append(sbs1port.add_pending_conns)
if options.no_print is not True: if options.no_print is not True:
printer = air_modes.output_print(cpr_dec, publisher) outputs.append(air_modes.output_print(my_position).output)
if options.multiplayer is not None: if options.multiplayer is not None:
[fghost, fgport] = options.multiplayer.split(':') [fghost, fgport] = options.multiplayer.split(':')
fgout = air_modes.output_flightgear(cpr_dec, fghost, int(fgport), publisher) fgout = air_modes.output_flightgear(my_position, fghost, int(fgport))
outputs.append(fgout.output)
if options.sbs1 is True: fg = adsb_rx_block(options, args, queue)
sbs1port = air_modes.output_sbs1(cpr_dec, 30003, publisher)
tb.run() if options.mlat_server is not None:
tb.close() mlat_queue = gr.msg_queue()
mlat_client = air_modes.mlat_client(mlat_queue, my_position, options.mlat_server, fg.time_source)
outputs.append(mlat_client.output) #output to server when we get a report
updates.append(mlat_client.get_mlat_positions) #check for replies from the server
#note: this means that get_mlat_positions and printmlat execute in the same thread.
#you could just have mlat_client spawn a thread to check its socket. might make more sense to do this.
mlat_outputs.append(printmlat)
runner = top_block_runner(fg)
relay.close() while 1:
try:
#handle the once-per-loop updates (check for mlat responses, add TCP conns to SBS-1 plugin, etc.)
for update in updates:
update()
#main message handler
if not queue.empty_p() :
while not queue.empty_p() :
msg = queue.delete_head() #blocking read
[data, ecc, reference, timestamp_int, timestamp_frac] = msg.to_string().split()
#error handling? creating a modes_reply can throw NoHandlerError, but you really want that to be handled by the output plugin in question.
msg_tuple = modes_report(modes_reply(long(data, 16)), long(ecc, 16), float(reference), stamp(int(timestamp_int), float(timestamp_frac)))
for out in outputs:
try:
out(msg_tuple)
except air_modes.ADSBError:
pass
if options.kml is not None: if mlat_queue:
kmlgen.close() if not mlat_queue.empty_p():
while not mlat_queue.empty_p():
msg = mlat_queue.delete_head()
for out in mlat_outputs:
try:
out(msg.to_string())
except air_modes.ADSBError:
pass
if __name__ == '__main__': if runner.done:
main() raise KeyboardInterrupt
else:
time.sleep(0.1)
except KeyboardInterrupt:
fg.stop()
runner = None
if options.kml is not None:
kmlgen.done = True
break

View File

@@ -0,0 +1,26 @@
INCLUDE(FindPkgConfig)
PKG_CHECK_MODULES(PC_GNURADIO_CORE gnuradio-core)
FIND_PATH(
GNURADIO_CORE_INCLUDE_DIRS
NAMES gr_random.h
HINTS $ENV{GNURADIO_CORE_DIR}/include/gnuradio
${PC_GNURADIO_CORE_INCLUDEDIR}
PATHS /usr/local/include/gnuradio
/usr/include/gnuradio
)
FIND_LIBRARY(
GNURADIO_CORE_LIBRARIES
NAMES gnuradio-core
HINTS $ENV{GNURADIO_CORE_DIR}/lib
${PC_GNURADIO_CORE_LIBDIR}
PATHS /usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
)
INCLUDE(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(GNURADIO_CORE DEFAULT_MSG GNURADIO_CORE_LIBRARIES GNURADIO_CORE_INCLUDE_DIRS)
MARK_AS_ADVANCED(GNURADIO_CORE_LIBRARIES GNURADIO_CORE_INCLUDE_DIRS)

View File

@@ -1,6 +0,0 @@
INCLUDE(FindPkgConfig)
PKG_CHECK_MODULES(GNURADIO_RUNTIME gnuradio-runtime)
INCLUDE(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(GNURADIO_RUNTIME DEFAULT_MSG GNURADIO_RUNTIME_LIBRARIES GNURADIO_RUNTIME_INCLUDE_DIRS)
MARK_AS_ADVANCED(GNURADIO_RUNTIME_LIBRARIES GNURADIO_RUNTIME_INCLUDE_DIRS)

View File

@@ -0,0 +1,26 @@
INCLUDE(FindPkgConfig)
PKG_CHECK_MODULES(PC_GRUEL gnuradio-core)
FIND_PATH(
GRUEL_INCLUDE_DIRS
NAMES gruel/attributes.h
HINTS $ENV{GRUEL_DIR}/include
${PC_GRUEL_INCLUDEDIR}
PATHS /usr/local/include
/usr/include
)
FIND_LIBRARY(
GRUEL_LIBRARIES
NAMES gruel
HINTS $ENV{GRUEL_DIR}/lib
${PC_GRUEL_LIBDIR}
PATHS /usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
)
INCLUDE(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(GRUEL DEFAULT_MSG GRUEL_LIBRARIES GRUEL_INCLUDE_DIRS)
MARK_AS_ADVANCED(GRUEL_LIBRARIES GRUEL_INCLUDE_DIRS)

View File

@@ -1,7 +1,6 @@
# Find PyQt4 # Find PyQt4
# ~~~~~~~~~~ # ~~~~~~~~~~
# Copyright (c) 2007-2008, Simon Edwards <simon@simonzone.com> # Copyright (c) 2007-2008, Simon Edwards <simon@simonzone.com>
# Copyright (c) 2012, Nicholas Corgan <nick.corgan@ettus.com>
# Redistribution and use is allowed according to the terms of the BSD license. # Redistribution and use is allowed according to the terms of the BSD license.
# For details see the accompanying COPYING-CMAKE-SCRIPTS file. # For details see the accompanying COPYING-CMAKE-SCRIPTS file.
# #
@@ -23,11 +22,10 @@
# #
# PYQT4_SIP_FLAGS - The SIP flags used to build PyQt. # PYQT4_SIP_FLAGS - The SIP flags used to build PyQt.
IF(EXISTS PYQT4_VERSION AND EXISTS PYUIC4_EXECUTABLE) IF(EXISTS PYQT4_VERSION)
# Already in cache, be silent # Already in cache, be silent
SET(PYQT4_FOUND TRUE) SET(PYQT4_FOUND TRUE)
SET(PYUIC4_FOUND TRUE) ELSE(EXISTS PYQT4_VERSION)
ELSE(EXISTS PYQT4_VERSION AND EXISTS PYUIC4_EXECUTABLE)
FIND_FILE(_find_pyqt_py FindPyQt.py PATHS ${CMAKE_MODULE_PATH}) FIND_FILE(_find_pyqt_py FindPyQt.py PATHS ${CMAKE_MODULE_PATH})
@@ -42,20 +40,14 @@ ELSE(EXISTS PYQT4_VERSION AND EXISTS PYUIC4_EXECUTABLE)
SET(PYQT4_FOUND TRUE) SET(PYQT4_FOUND TRUE)
ENDIF(pyqt_config) ENDIF(pyqt_config)
FIND_PROGRAM(PYUIC4_EXECUTABLE NAMES pyuic4) IF(PYQT4_FOUND)
IF(PYUIC4_EXECUTABLE)
SET(PYUIC4_FOUND TRUE)
ENDIF(PYUIC4_EXECUTABLE)
IF(PYQT4_FOUND AND PYUIC4_FOUND)
IF(NOT PYQT4_FIND_QUIETLY) IF(NOT PYQT4_FIND_QUIETLY)
MESSAGE(STATUS "Found PyQt4 version: ${PYQT4_VERSION_STR}") MESSAGE(STATUS "Found PyQt4 version: ${PYQT4_VERSION_STR}")
MESSAGE(STATUS "Found pyuic4: ${PYUIC4_EXECUTABLE}")
ENDIF(NOT PYQT4_FIND_QUIETLY) ENDIF(NOT PYQT4_FIND_QUIETLY)
ELSE(PYQT4_FOUND AND PYUIC4_FOUND) ELSE(PYQT4_FOUND)
IF(PYQT4_FIND_REQUIRED) IF(PYQT4_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find Python") MESSAGE(FATAL_ERROR "Could not find Python")
ENDIF(PYQT4_FIND_REQUIRED) ENDIF(PYQT4_FIND_REQUIRED)
ENDIF(PYQT4_FOUND AND PYUIC4_FOUND) ENDIF(PYQT4_FOUND)
ENDIF(EXISTS PYQT4_VERSION AND EXISTS PYUIC4_EXECUTABLE) ENDIF(EXISTS PYQT4_VERSION)

View File

@@ -1,56 +0,0 @@
# - Find zeromq libraries
# This module finds zeromq if it is installed and determines where the
# include files and libraries are. It also determines what the name of
# the library is. This code sets the following variables:
#
# ZEROMQ_FOUND - have the zeromq libs been found
# ZEROMQ_LIBRARIES - path to the zeromq library
# ZEROMQ_INCLUDE_DIRS - path to where zmq.h is found
# ZEROMQ_DEBUG_LIBRARIES - path to the debug library
#INCLUDE(CMakeFindFrameworks)
# Search for the zeromq framework on Apple.
#CMAKE_FIND_FRAMEWORKS(ZeroMQ)
IF(WIN32)
FIND_LIBRARY(ZEROMQ_DEBUG_LIBRARY
NAMES libzmq_d zmq_d
PATHS
${ZEROMQ_LIBRARIES}
)
ENDIF(WIN32)
FIND_LIBRARY(ZEROMQ_LIBRARY
NAMES libzmq zmq
PATHS
${ZEROMQ_LIBRARIES}
${NSCP_LIBRARYDIR}
)
# IF(ZeroMQ_FRAMEWORKS AND NOT ZEROMQ_INCLUDE_DIR)
# FOREACH(dir ${ZeroMQ_FRAMEWORKS})
# SET(ZEROMQ_FRAMEWORK_INCLUDES ${ZEROMQ_FRAMEWORK_INCLUDES}
# ${dir}/Versions/${_CURRENT_VERSION}/include/zeromq${_CURRENT_VERSION})
# ENDFOREACH(dir)
# ENDIF(ZeroMQ_FRAMEWORKS AND NOT ZEROMQ_INCLUDE_DIR)
FIND_PATH(ZEROMQ_INCLUDE_DIR
NAMES zmq.hpp
PATHS
# ${ZEROMQ_FRAMEWORK_INCLUDES}
${ZEROMQ_INCLUDE_DIRS}
${NSCP_INCLUDEDIR}
${ZEROMQ_INCLUDE_DIR}
)
MARK_AS_ADVANCED(
ZEROMQ_DEBUG_LIBRARY
ZEROMQ_LIBRARY
ZEROMQ_INCLUDE_DIR
)
SET(ZEROMQ_INCLUDE_DIRS "${ZEROMQ_INCLUDE_DIR}")
SET(ZEROMQ_LIBRARIES "${ZEROMQ_LIBRARY}")
SET(ZEROMQ_DEBUG_LIBRARIES "${ZEROMQ_DEBUG_LIBRARY}")
INCLUDE(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(ZeroMQ DEFAULT_MSG ZEROMQ_LIBRARIES ZEROMQ_INCLUDE_DIRS)

View File

@@ -1,99 +0,0 @@
# Copyright 2010-2011 Free Software Foundation, Inc.
#
# This file is part of GNU Radio
#
# GNU Radio 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.
#
# GNU Radio 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 GNU Radio; see the file COPYING. If not, write to
# the Free Software Foundation, Inc., 51 Franklin Street,
# Boston, MA 02110-1301, USA.
if(DEFINED __INCLUDED_GR_BOOST_CMAKE)
return()
endif()
set(__INCLUDED_GR_BOOST_CMAKE TRUE)
########################################################################
# Setup Boost and handle some system specific things
########################################################################
set(BOOST_REQUIRED_COMPONENTS
date_time
program_options
filesystem
system
thread
)
if(UNIX AND NOT BOOST_ROOT AND EXISTS "/usr/lib64")
list(APPEND BOOST_LIBRARYDIR "/usr/lib64") #fedora 64-bit fix
endif(UNIX AND NOT BOOST_ROOT AND EXISTS "/usr/lib64")
if(MSVC)
set(BOOST_REQUIRED_COMPONENTS ${BOOST_REQUIRED_COMPONENTS} chrono)
if (NOT DEFINED BOOST_ALL_DYN_LINK)
set(BOOST_ALL_DYN_LINK TRUE)
endif()
set(BOOST_ALL_DYN_LINK "${BOOST_ALL_DYN_LINK}" CACHE BOOL "boost enable dynamic linking")
if(BOOST_ALL_DYN_LINK)
add_definitions(-DBOOST_ALL_DYN_LINK) #setup boost auto-linking in msvc
else(BOOST_ALL_DYN_LINK)
unset(BOOST_REQUIRED_COMPONENTS) #empty components list for static link
endif(BOOST_ALL_DYN_LINK)
endif(MSVC)
find_package(Boost "1.35" COMPONENTS ${BOOST_REQUIRED_COMPONENTS})
# This does not allow us to disable specific versions. It is used
# internally by cmake to know the formation newer versions. As newer
# Boost version beyond what is shown here are produced, we must extend
# this list. To disable Boost versions, see below.
set(Boost_ADDITIONAL_VERSIONS
"1.35.0" "1.35" "1.36.0" "1.36" "1.37.0" "1.37" "1.38.0" "1.38" "1.39.0" "1.39"
"1.40.0" "1.40" "1.41.0" "1.41" "1.42.0" "1.42" "1.43.0" "1.43" "1.44.0" "1.44"
"1.45.0" "1.45" "1.46.0" "1.46" "1.47.0" "1.47" "1.48.0" "1.48" "1.49.0" "1.49"
"1.50.0" "1.50" "1.51.0" "1.51" "1.52.0" "1.52" "1.53.0" "1.53" "1.54.0" "1.54"
"1.55.0" "1.55" "1.56.0" "1.56" "1.57.0" "1.57" "1.58.0" "1.58" "1.59.0" "1.59"
"1.60.0" "1.60" "1.61.0" "1.61" "1.62.0" "1.62" "1.63.0" "1.63" "1.64.0" "1.64"
"1.65.0" "1.65" "1.66.0" "1.66" "1.67.0" "1.67" "1.68.0" "1.68" "1.69.0" "1.69"
)
# Boost 1.52 disabled, see https://svn.boost.org/trac/boost/ticket/7669
# Similar problems with Boost 1.46 and 1.47.
OPTION(ENABLE_BAD_BOOST "Enable known bad versions of Boost" OFF)
if(ENABLE_BAD_BOOST)
MESSAGE(STATUS "Enabling use of known bad versions of Boost.")
endif(ENABLE_BAD_BOOST)
# For any unsuitable Boost version, add the version number below in
# the following format: XXYYZZ
# Where:
# XX is the major version ('10' for version 1)
# YY is the minor version number ('46' for 1.46)
# ZZ is the patcher version number (typically just '00')
set(Boost_NOGO_VERSIONS
104600 104601 104700 105200
)
foreach(ver ${Boost_NOGO_VERSIONS})
if(${Boost_VERSION} EQUAL ${ver})
if(NOT ENABLE_BAD_BOOST)
MESSAGE(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION}). Disabling.")
set(Boost_FOUND FALSE)
else(NOT ENABLE_BAD_BOOST)
MESSAGE(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION}). Continuing anyway.")
set(Boost_FOUND TRUE)
endif(NOT ENABLE_BAD_BOOST)
endif(${Boost_VERSION} EQUAL ${ver})
endforeach(ver)

View File

@@ -22,7 +22,7 @@
#ifndef INCLUDED_AIR_MODES_API_H #ifndef INCLUDED_AIR_MODES_API_H
#define INCLUDED_AIR_MODES_API_H #define INCLUDED_AIR_MODES_API_H
#include <gnuradio/attributes.h> #include <gruel/attributes.h>
#ifdef AIR_MODES_EXPORTS #ifdef AIR_MODES_EXPORTS
# define AIR_MODES_API __GR_ATTR_EXPORT # define AIR_MODES_API __GR_ATTR_EXPORT

View File

@@ -23,7 +23,7 @@
#ifndef INCLUDED_AIR_MODES_PREAMBLE_H #ifndef INCLUDED_AIR_MODES_PREAMBLE_H
#define INCLUDED_AIR_MODES_PREAMBLE_H #define INCLUDED_AIR_MODES_PREAMBLE_H
#include <gnuradio/block.h> #include <gr_block.h>
#include <air_modes_api.h> #include <air_modes_api.h>
class air_modes_preamble; class air_modes_preamble;
@@ -35,7 +35,7 @@ AIR_MODES_API air_modes_preamble_sptr air_make_modes_preamble(int channel_rate,
* \brief mode select preamble detection * \brief mode select preamble detection
* \ingroup block * \ingroup block
*/ */
class AIR_MODES_API air_modes_preamble : public gr::block class AIR_MODES_API air_modes_preamble : public gr_block
{ {
private: private:
friend air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db); friend air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db);
@@ -49,7 +49,7 @@ private:
float d_threshold_db; float d_threshold_db;
float d_threshold; float d_threshold;
pmt::pmt_t d_me, d_key; pmt::pmt_t d_me, d_key;
gr::tag_t d_timestamp; gr_tag_t d_timestamp;
double d_secs_per_sample; double d_secs_per_sample;
public: public:
@@ -57,10 +57,6 @@ public:
gr_vector_int &ninput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items); gr_vector_void_star &output_items);
void set_rate(int channel_rate);
void set_threshold(float threshold_db);
float get_threshold(void);
}; };
#endif /* INCLUDED_AIR_MODES_PREAMBLE_H */ #endif /* INCLUDED_AIR_MODES_PREAMBLE_H */

View File

@@ -23,31 +23,32 @@
#ifndef INCLUDED_AIR_MODES_slicer_H #ifndef INCLUDED_AIR_MODES_slicer_H
#define INCLUDED_AIR_MODES_slicer_H #define INCLUDED_AIR_MODES_slicer_H
#include <gnuradio/sync_block.h> #include <gr_sync_block.h>
#include <gnuradio/msg_queue.h> #include <gr_msg_queue.h>
#include <air_modes_api.h> #include <air_modes_api.h>
class air_modes_slicer; class air_modes_slicer;
typedef boost::shared_ptr<air_modes_slicer> air_modes_slicer_sptr; typedef boost::shared_ptr<air_modes_slicer> air_modes_slicer_sptr;
AIR_MODES_API air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr::msg_queue::sptr queue); AIR_MODES_API air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr_msg_queue_sptr queue);
/*! /*!
* \brief mode select slicer detection * \brief mode select slicer detection
* \ingroup block * \ingroup block
*/ */
class AIR_MODES_API air_modes_slicer : public gr::sync_block class AIR_MODES_API air_modes_slicer : public gr_sync_block
{ {
private: private:
friend air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr::msg_queue::sptr queue); friend air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr_msg_queue_sptr queue);
air_modes_slicer(int channel_rate, gr::msg_queue::sptr queue); air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue);
int d_check_width; int d_check_width;
int d_chip_rate; int d_chip_rate;
int d_samples_per_chip; int d_samples_per_chip;
int d_samples_per_symbol; int d_samples_per_symbol;
gr::msg_queue::sptr d_queue; gr_msg_queue_sptr d_queue;
std::ostringstream d_payload; unsigned char d_lowconfbits[24];
unsigned char d_data[14];
public: public:
int work (int noutput_items, int work (int noutput_items,

View File

@@ -24,20 +24,6 @@
#define AIR_MODES_TYPES_H #define AIR_MODES_TYPES_H
typedef enum { No_Packet = 0, Short_Packet = 1, Fruited_Packet = 2, Long_Packet = 3 } framer_packet_type; typedef enum { No_Packet = 0, Short_Packet = 1, Fruited_Packet = 2, Long_Packet = 3 } framer_packet_type;
typedef enum { No_Error = 0, Solution_Found, Too_Many_LCBs, No_Solution, Multiple_Solutions } bruteResultTypeDef;
struct modes_packet {
unsigned char data[14];
// unsigned char confidence[14]; //112 bits of boolean high/low confidence data for each bit
unsigned char lowconfbits[24]; //positions of low confidence bits within the packet
unsigned long crc;
unsigned int numlowconf;
framer_packet_type type; //what length packet are we
unsigned int message_type;
float reference_level;
double timestamp;
};
struct slice_result_t { struct slice_result_t {
bool decision; bool decision;

View File

@@ -24,7 +24,4 @@
#define INCLUDED_MODES_CRC_H #define INCLUDED_MODES_CRC_H
extern const unsigned int modes_crc_table[112]; extern const unsigned int modes_crc_table[112];
int modes_check_crc(unsigned char data[], int length); int modes_check_crc(unsigned char data[], int length);
bruteResultTypeDef modes_ec_brute(modes_packet &err_packet);
unsigned next_set_of_n_elements(unsigned x);
#endif #endif

View File

@@ -27,7 +27,7 @@ add_library(air_modes SHARED
air_modes_slicer.cc air_modes_slicer.cc
modes_crc.cc modes_crc.cc
) )
target_link_libraries(air_modes ${Boost_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}) target_link_libraries(air_modes ${Boost_LIBRARIES} ${GRUEL_LIBRARIES} ${GNURADIO_CORE_LIBRARIES})
set_target_properties(air_modes PROPERTIES DEFINE_SYMBOL "AIR_MODES_EXPORTS") set_target_properties(air_modes PROPERTIES DEFINE_SYMBOL "AIR_MODES_EXPORTS")
set_target_properties(air_modes PROPERTIES SOVERSION "${gr-gr-air-modes_VERSION_MAJOR}") set_target_properties(air_modes PROPERTIES SOVERSION "${gr-gr-air-modes_VERSION_MAJOR}")
set_target_properties(air_modes PROPERTIES VERSION "${gr-gr-air-modes_VERSION_MAJOR}.${gr-gr-air-modes_VERSION_MINOR}") set_target_properties(air_modes PROPERTIES VERSION "${gr-gr-air-modes_VERSION_MAJOR}.${gr-gr-air-modes_VERSION_MINOR}")

View File

@@ -1,6 +1,5 @@
/* /*
# Copyright 2010 Nick Foster # Copyright 2010 Nick Foster
# Copyright 2013 Nicholas Corgan
# #
# This file is part of gr-air-modes # This file is part of gr-air-modes
# #
@@ -25,12 +24,11 @@
#include "config.h" #include "config.h"
#endif #endif
#include <ciso646>
#include <air_modes_preamble.h> #include <air_modes_preamble.h>
#include <gnuradio/io_signature.h> #include <gr_io_signature.h>
#include <string.h> #include <string.h>
#include <iostream> #include <iostream>
#include <gnuradio/tags.h> #include <gr_tags.h>
air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db) air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshold_db)
{ {
@@ -38,9 +36,9 @@ air_modes_preamble_sptr air_make_modes_preamble(int channel_rate, float threshol
} }
air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) : air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
gr::block ("modes_preamble", gr_block ("modes_preamble",
gr::io_signature::make2 (2, 2, sizeof(float), sizeof(float)), //stream 0 is received data, stream 1 is moving average for reference gr_make_io_signature2 (2, 2, sizeof(float), sizeof(float)), //stream 0 is received data, stream 1 is moving average for reference
gr::io_signature::make (1, 1, sizeof(float))) //the output packets gr_make_io_signature (1, 1, sizeof(float))) //the output packets
{ {
d_chip_rate = 2000000; //2Mchips per second d_chip_rate = 2000000; //2Mchips per second
d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work d_samples_per_chip = channel_rate / d_chip_rate; //must be integer number of samples per chip to work
@@ -53,8 +51,8 @@ air_modes_preamble::air_modes_preamble(int channel_rate, float threshold_db) :
std::stringstream str; std::stringstream str;
str << name() << unique_id(); str << name() << unique_id();
d_me = pmt::string_to_symbol(str.str()); d_me = pmt::pmt_string_to_symbol(str.str());
d_key = pmt::string_to_symbol("preamble_found"); d_key = pmt::pmt_string_to_symbol("preamble_found");
set_history(d_samples_per_symbol); set_history(d_samples_per_symbol);
} }
@@ -80,20 +78,31 @@ static double correlate_preamble(const float *in, int samples_per_chip) {
return corr; return corr;
} }
//todo: make it return a pair of some kind, otherwise you can lose precision //takes an rx tag and issues a tag offset appropriately for the preamble
static double tag_to_timestamp(gr::tag_t tstamp, uint64_t abs_sample_cnt, double secs_per_sample) { static const pmt::pmt_t offset_stamp(const gr_tag_t &tstamp, const uint64_t abs_sample_cnt, const double secs_per_sample) {
uint64_t ts_sample, last_whole_stamp; uint64_t ts_sample, last_whole_stamp;
double last_frac_stamp; double last_frac_stamp;
if(tstamp.key == NULL || pmt::symbol_to_string(tstamp.key) != "rx_time") return 0; if(tstamp.key == NULL || pmt::pmt_symbol_to_string(tstamp.key) != "rx_time") {
last_whole_stamp = 0;
last_whole_stamp = pmt::to_uint64(pmt::tuple_ref(tstamp.value, 0)); last_frac_stamp = 0;
last_frac_stamp = pmt::to_double(pmt::tuple_ref(tstamp.value, 1)); ts_sample = 0;
ts_sample = tstamp.offset; } else {
last_whole_stamp = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tstamp.value, 0));
last_frac_stamp = pmt::pmt_to_double(pmt::pmt_tuple_ref(tstamp.value, 1));
ts_sample = tstamp.offset;
}
double tstime = double(abs_sample_cnt * secs_per_sample) + last_whole_stamp + last_frac_stamp; double fractime = double(abs_sample_cnt * secs_per_sample) + last_frac_stamp;
if(0) std::cout << "HEY WE GOT A STAMP AT " << tstime << " TICKS AT SAMPLE " << ts_sample << " ABS SAMPLE CNT IS " << abs_sample_cnt << std::endl; last_whole_stamp += int(fractime);
return tstime; fractime -= int(fractime);
const pmt::pmt_t newval = pmt::pmt_make_tuple(
pmt::pmt_from_uint64(last_whole_stamp),
pmt::pmt_from_double(fractime)
);
return newval;
} }
int air_modes_preamble::general_work(int noutput_items, int air_modes_preamble::general_work(int noutput_items,
@@ -122,8 +131,8 @@ int air_modes_preamble::general_work(int noutput_items,
}; };
uint64_t abs_sample_cnt = nitems_read(0); uint64_t abs_sample_cnt = nitems_read(0);
std::vector<gr::tag_t> tstamp_tags; std::vector<gr_tag_t> tstamp_tags;
get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::string_to_symbol("rx_time")); get_tags_in_range(tstamp_tags, 0, abs_sample_cnt, abs_sample_cnt + ninputs, pmt::pmt_string_to_symbol("rx_time"));
//tags.back() is the most recent timestamp, then. //tags.back() is the most recent timestamp, then.
if(tstamp_tags.size() > 0) { if(tstamp_tags.size() > 0) {
d_timestamp = tstamp_tags.back(); d_timestamp = tstamp_tags.back();
@@ -188,16 +197,14 @@ int air_modes_preamble::general_work(int noutput_items,
integrate_and_dump(out, &in[i], 240, d_samples_per_chip); integrate_and_dump(out, &in[i], 240, d_samples_per_chip);
} }
//get the timestamp of the preamble const pmt::pmt_t new_pmt = offset_stamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
double tstamp = tag_to_timestamp(d_timestamp, abs_sample_cnt + i, d_secs_per_sample);
//now tag the preamble //now tag the preamble
add_item_tag(0, //stream ID add_item_tag(0, //stream ID
nitems_written(0), //sample nitems_written(0), //sample
d_key, //frame_info d_key, //frame_info
pmt::from_double(tstamp), new_pmt,
d_me //block src id d_me //block src id
); );
//std::cout << "PREAMBLE" << std::endl; //std::cout << "PREAMBLE" << std::endl;

View File

@@ -1,6 +1,5 @@
/* /*
# Copyright 2010 Nick Foster # Copyright 2010 Nick Foster
# Copyright 2013 Nicholas Corgan
# #
# This file is part of gr-air-modes # This file is part of gr-air-modes
# #
@@ -25,15 +24,14 @@
#include "config.h" #include "config.h"
#endif #endif
#include <ciso646>
#include <air_modes_slicer.h> #include <air_modes_slicer.h>
#include <gnuradio/io_signature.h> #include <gr_io_signature.h>
#include <air_modes_types.h> #include <air_modes_types.h>
#include <sstream> #include <sstream>
#include <iomanip> #include <iomanip>
#include <modes_crc.h> #include <modes_crc.h>
#include <iostream> #include <iostream>
#include <gnuradio/tags.h> #include <gr_tags.h>
extern "C" extern "C"
{ {
@@ -41,15 +39,15 @@ extern "C"
#include <string.h> #include <string.h>
} }
air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr::msg_queue::sptr queue) air_modes_slicer_sptr air_make_modes_slicer(int channel_rate, gr_msg_queue_sptr queue)
{ {
return air_modes_slicer_sptr (new air_modes_slicer(channel_rate, queue)); return air_modes_slicer_sptr (new air_modes_slicer(channel_rate, queue));
} }
air_modes_slicer::air_modes_slicer(int channel_rate, gr::msg_queue::sptr queue) : air_modes_slicer::air_modes_slicer(int channel_rate, gr_msg_queue_sptr queue) :
gr::sync_block ("modes_slicer", gr_sync_block ("modes_slicer",
gr::io_signature::make (1, 1, sizeof(float)), //stream 0 is received data, stream 1 is binary preamble detector output gr_make_io_signature (1, 1, sizeof(float)), //stream 0 is received data, stream 1 is binary preamble detector output
gr::io_signature::make (0, 0, 0) ) gr_make_io_signature (0, 0, 0) )
{ {
//initialize private data here //initialize private data here
d_chip_rate = 2000000; //2Mchips per second d_chip_rate = 2000000; //2Mchips per second
@@ -107,91 +105,86 @@ int air_modes_slicer::work(int noutput_items,
if(0) std::cout << "Slicer called with " << size << " samples" << std::endl; if(0) std::cout << "Slicer called with " << size << " samples" << std::endl;
std::vector<gr::tag_t> tags; std::vector<gr_tag_t> tags;
uint64_t abs_sample_cnt = nitems_read(0); uint64_t abs_sample_cnt = nitems_read(0);
get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + size, pmt::string_to_symbol("preamble_found")); get_tags_in_range(tags, 0, abs_sample_cnt, abs_sample_cnt + size, pmt::pmt_string_to_symbol("preamble_found"));
std::vector<gr::tag_t>::iterator tag_iter; std::vector<gr_tag_t>::iterator tag_iter;
for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) { for(tag_iter = tags.begin(); tag_iter != tags.end(); tag_iter++) {
uint64_t i = tag_iter->offset - abs_sample_cnt; uint64_t i = tag_iter->offset - abs_sample_cnt;
modes_packet rx_packet;
memset(&rx_packet.data, 0x00, 14 * sizeof(unsigned char)); memset(&d_data, 0x00, 14 * sizeof(unsigned char));
memset(&rx_packet.lowconfbits, 0x00, 24 * sizeof(unsigned char)); memset(&d_lowconfbits, 0x00, 24 * sizeof(unsigned char));
rx_packet.numlowconf = 0; unsigned int numlowconf = 0;
//let's use the preamble to get a reference level for the packet //let's use the preamble to get a reference level for the packet
//fixme: a better thing to do is create a bi-level avg 1 and avg 0 //fixme: a better thing to do is create a bi-level avg 1 and avg 0
//through simple statistics, then take the median for your slice level //through simple statistics, then take the median for your slice level
//this won't improve decoding but will improve confidence //this won't improve decoding but will improve confidence
rx_packet.reference_level = (in[i] double reference_level = (in[i]
+ in[i+2] + in[i+2]
+ in[i+7] + in[i+7]
+ in[i+9]) / 4.0; + in[i+9]) / 4.0;
i += 16; //move on up to the first bit of the packet data i += 16; //move on up to the first bit of the packet data
//now let's slice the header so we can determine if it's a short pkt or a long pkt //now let's slice the header so we can determine if it's a short pkt or a long pkt
unsigned char pkt_hdr = 0; unsigned char pkt_hdr = 0;
for(int j=0; j < 5; j++) { for(int j=0; j < 5; j++) {
slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], rx_packet.reference_level); slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], reference_level);
if(slice_result.decision) pkt_hdr += 1 << (4-j); if(slice_result.decision) pkt_hdr += 1 << (4-j);
} }
if(pkt_hdr == 16 or pkt_hdr == 17 or pkt_hdr == 20 or pkt_hdr == 21) rx_packet.type = Long_Packet; framer_packet_type type;
else rx_packet.type = Short_Packet; if(pkt_hdr == 16 or pkt_hdr == 17 or pkt_hdr == 20 or pkt_hdr == 21) type = Long_Packet;
int packet_length = (rx_packet.type == framer_packet_type(Short_Packet)) ? 56 : 112; else type = Short_Packet;
int packet_length = (type == Short_Packet) ? 56 : 112;
//it's slice time! //it's slice time!
//TODO: don't repeat your work here, you already have the first 5 bits //TODO: don't repeat your work here, you already have the first 5 bits
slice_result_t slice_result;
for(int j = 0; j < packet_length; j++) { for(int j = 0; j < packet_length; j++) {
slice_result_t slice_result = slicer(in[i+j*2], in[i+j*2+1], rx_packet.reference_level); slice_result = slicer(in[i+j*2], in[i+j*2+1], reference_level);
//put the data into the packet //put the data into the packet
if(slice_result.decision) { if(slice_result.decision) {
rx_packet.data[j/8] += 1 << (7-(j%8)); d_data[j/8] += 1 << (7-(j%8));
} }
//put the confidence decision into the packet //put the confidence decision into the packet
if(slice_result.confidence) { if(slice_result.confidence) {
//rx_packet.confidence[j/8] += 1 << (7-(j%8)); //d_confidence[j/8] += 1 << (7-(j%8));
} else { } else {
if(rx_packet.numlowconf < 24) rx_packet.lowconfbits[rx_packet.numlowconf++] = j; if(numlowconf < 24) d_lowconfbits[numlowconf++] = j;
} }
} }
/******************** BEGIN TIMESTAMP BS ******************/ uint64_t timestamp_secs = pmt::pmt_to_uint64(pmt::pmt_tuple_ref(tag_iter->value, 0));
rx_packet.timestamp = pmt::to_double(tag_iter->value); double timestamp_frac = pmt::pmt_to_double(pmt::pmt_tuple_ref(tag_iter->value, 1));
/******************* END TIMESTAMP BS *********************/
//increment for the next round
//here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC. //here you might want to traverse the whole packet and if you find all 0's, just toss it. don't know why these packets turn up, but they pass ECC.
bool zeroes = 1; bool zeroes = 1;
for(int m = 0; m < 14; m++) { for(int m = 0; m < 14; m++) {
if(rx_packet.data[m]) zeroes = 0; if(d_data[m]) zeroes = 0;
} }
if(zeroes) {continue;} //toss it if(zeroes) {continue;} //toss it
rx_packet.message_type = (rx_packet.data[0] >> 3) & 0x1F; //get the message type to make decisions on ECC methods unsigned int message_type = (d_data[0] >> 3) & 0x1F; //get the message type to make decisions on ECC methods
if(rx_packet.type == Short_Packet && rx_packet.message_type != 11 && rx_packet.numlowconf > 0) {continue;} if(type == Short_Packet && message_type != 11 && numlowconf > 0) {continue;}
if(rx_packet.message_type == 11 && rx_packet.numlowconf >= 10) {continue;} if(message_type == 11 && numlowconf >= 10) {continue;}
rx_packet.crc = modes_check_crc(rx_packet.data, packet_length); unsigned long crc = modes_check_crc(d_data, packet_length);
//crc for packets that aren't type 11 or type 17 is encoded with the transponder ID, which we don't know //crc for packets that aren't type 11 or type 17 is encoded with the transponder ID, which we don't know
//therefore we toss 'em if there's syndrome //we forward them if they have no low-confidence bits (see above), but this still lets some crap through.
//crc for the other short packets is usually nonzero, so they can't really be trusted that far if(crc && (message_type == 11 || message_type == 17)) {continue;}
if(rx_packet.crc && (rx_packet.message_type == 11 || rx_packet.message_type == 17)) {continue;} std::ostringstream payload;
d_payload.str("");
for(int m = 0; m < packet_length/8; m++) { for(int m = 0; m < packet_length/8; m++) {
d_payload << std::hex << std::setw(2) << std::setfill('0') << unsigned(rx_packet.data[m]); payload << std::hex << std::setw(2) << std::setfill('0') << unsigned(d_data[m]);
} }
d_payload << " " << std::setw(6) << rx_packet.crc << " " << std::dec << rx_packet.reference_level payload << " " << std::setw(6) << crc << " " << std::dec << reference_level
<< " " << std::setprecision(10) << std::setw(10) << rx_packet.timestamp; << " " << timestamp_secs << " " << std::setprecision(20) << timestamp_frac;
gr::message::sptr msg = gr::message::make_from_string(std::string(d_payload.str())); gr_message_sptr msg = gr_make_message_from_string(std::string(payload.str()));
d_queue->handle(msg); d_queue->handle(msg);
} }
if(0) std::cout << "Slicer consumed " << size << ", returned " << size << std::endl;
return size; return size;
} }

View File

@@ -34,21 +34,19 @@ GR_PYTHON_INSTALL(
altitude.py altitude.py
az_map.py az_map.py
cpr.py cpr.py
html_template.py
mlat.py mlat.py
mlat_client.py
exceptions.py exceptions.py
flightgear.py flightgear.py
gui_model.py gui_model.py
kml.py kml.py
parse.py parse.py
msprint.py msprint.py
radio.py
raw_server.py raw_server.py
rx_path.py rx_path.py
sbs1.py sbs1.py
sql.py sql.py
types.py types.py
zmq_socket.py
Quaternion.py Quaternion.py
DESTINATION ${GR_PYTHON_DIR}/air_modes DESTINATION ${GR_PYTHON_DIR}/air_modes
) )

View File

@@ -52,20 +52,16 @@ from air_modes_swig import *
# import any pure python here # import any pure python here
# #
from rx_path import rx_path from rx_path import rx_path
from zmq_socket import zmq_pubsub_iface from parse import parse,modes_reply
from parse import *
from msprint import output_print from msprint import output_print
from sql import output_sql from sql import output_sql
from sbs1 import output_sbs1 from sbs1 import output_sbs1
from kml import output_kml, output_jsonp from kml import output_kml
from raw_server import raw_server from raw_server import raw_server
from radio import modes_radio from mlat_client import mlat_client
from exceptions import * from exceptions import *
from az_map import * from az_map import *
from types import * from types import *
from altitude import *
from cpr import cpr_decoder
from html_template import html_template
#this is try/excepted in case the user doesn't have numpy installed #this is try/excepted in case the user doesn't have numpy installed
try: try:
from flightgear import output_flightgear from flightgear import output_flightgear

View File

@@ -26,7 +26,6 @@ from PyQt4 import QtCore, QtGui
import threading import threading
import math import math
import air_modes import air_modes
from air_modes.exceptions import *
# model has max range vs. azimuth in n-degree increments # model has max range vs. azimuth in n-degree increments
@@ -169,30 +168,30 @@ class az_map(QtGui.QWidget):
self.ringsize = ringsize self.ringsize = ringsize
self.drawPath() self.drawPath()
class az_map_output: class az_map_output(air_modes.parse):
def __init__(self, cprdec, model, pub): def __init__(self, mypos, model):
self._cpr = cprdec air_modes.parse.__init__(self, mypos)
self.model = model self.model = model
pub.subscribe("type17_dl", self.output)
def output(self, msg): def output(self, msg):
try: [data, ecc, reference, timestamp] = msg.split()
msgtype = msg.data["df"] data = air_modes.modes_reply(long(data, 16))
now = time.time() ecc = long(ecc, 16)
rssi = 10.*math.log10(float(reference))
msgtype = data["df"]
now = time.time()
if msgtype == 17: if msgtype == 17:
icao = msg.data["aa"] icao = data["aa"]
subtype = msg.data["ftc"] subtype = data["ftc"]
distance, altitude, bearing = [0,0,0] distance, altitude, bearing = [0,0,0]
if 5 <= subtype <= 8: if 5 <= subtype <= 8:
(ground_track, decoded_lat, decoded_lon, distance, bearing) = air_modes.parseBDS06(msg.data, self._cpr) (ground_track, decoded_lat, decoded_lon, distance, bearing) = self.parseBDS06(data)
altitude = 0 altitude = 0
elif 9 <= subtype <= 18: elif 9 <= subtype <= 18:
(altitude, decoded_lat, decoded_lon, distance, bearing) = air_modes.parseBDS05(msg.data, self._cpr) (altitude, decoded_lat, decoded_lon, distance, bearing) = self.parseBDS05(data)
self.model.addRecord(bearing, altitude, distance) self.model.addRecord(bearing, altitude, distance)
except ADSBError:
pass
############################## ##############################

View File

@@ -188,7 +188,7 @@ class cpr_decoder:
self.evenlist_sfc = {} self.evenlist_sfc = {}
self.oddlist_sfc = {} self.oddlist_sfc = {}
def set_location(self, new_location): def set_location(new_location):
self.my_location = new_location self.my_location = new_location
def weed_poslists(self): def weed_poslists(self):

View File

@@ -24,12 +24,12 @@ class ADSBError(Exception):
class MetricAltError(ADSBError): class MetricAltError(ADSBError):
pass pass
class ParserError(ADSBError): class ParserError(ADSBError):
pass pass
class NoHandlerError(ADSBError): class NoHandlerError(ADSBError):
def __init__(self, msgtype=None): def __init__(self, msgtype):
self.msgtype = msgtype self.msgtype = msgtype
class MlatNonConvergeError(ADSBError): class MlatNonConvergeError(ADSBError):
@@ -44,4 +44,3 @@ class CPRBoundaryStraddleError(CPRNoPositionError):
class FieldNotInPacket(ParserError): class FieldNotInPacket(ParserError):
def __init__(self, item): def __init__(self, item):
self.item = item self.item = item

View File

@@ -14,19 +14,18 @@ from Quaternion import Quat
import numpy import numpy
from air_modes.exceptions import * from air_modes.exceptions import *
class output_flightgear: class output_flightgear(air_modes.parse):
def __init__(self, cprdec, hostname, port, pub): def __init__(self, localpos, hostname, port):
air_modes.parse.__init__(self, localpos)
self.hostname = hostname self.hostname = hostname
self.port = port self.port = port
self.localpos = localpos self.localpos = localpos
self.positions = {} self.positions = {}
self.velocities = {} self.velocities = {}
self.callsigns = {} self.callsigns = {}
self._cpr = cprdec
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.connect((self.hostname, self.port)) self.sock.connect((self.hostname, self.port))
pub.subscribe("type17_dl", output)
def output(self, msg): def output(self, msg):
@@ -36,26 +35,26 @@ class output_flightgear:
icao24 = msg.data["aa"] icao24 = msg.data["aa"]
bdsreg = msg.data["me"].get_type() bdsreg = msg.data["me"].get_type()
if bdsreg == 0x08: #ident packet if bdsreg == 0x08: #ident packet
(ident, actype) = air_modes.parseBDS08(data) (ident, actype) = self.parseBDS08(msg.data)
#select model based on actype #select model based on actype
self.callsigns[icao24] = [ident, actype] self.callsigns[icao24] = [ident, actype]
elif bdsreg == 0x06: #BDS0,6 pos elif bdsreg == 0x06: #BDS0,6 pos
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS06(data, self._cpr) [ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(msg.data)
self.positions[icao24] = [decoded_lat, decoded_lon, 0] self.positions[icao24] = [decoded_lat, decoded_lon, 0]
self.update(icao24) self.update(icao24)
elif bdsreg == 0x05: #BDS0,5 pos elif bdsreg == 0x05: #BDS0,5 pos
[altitude, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS05(data, self._cpr) [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(msg.data)
self.positions[icao24] = [decoded_lat, decoded_lon, altitude] self.positions[icao24] = [decoded_lat, decoded_lon, altitude]
self.update(icao24) self.update(icao24)
elif bdsreg == 0x09: #velocity elif bdsreg == 0x09: #velocity
subtype = data["bds09"].get_type() subtype = data["bds09"].get_type()
if subtype == 0: if subtype == 0:
[velocity, heading, vert_spd, turnrate] = air_modes.parseBDS09_0(data) [velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(msg.data)
elif subtype == 1: elif subtype == 1:
[velocity, heading, vert_spd] = air_modes.parseBDS09_1(data) [velocity, heading, vert_spd] = self.parseBDS09_1(msg.data)
turnrate = 0 turnrate = 0
else: else:
return return

View File

@@ -22,11 +22,9 @@
# This file contains data models, view delegates, and associated classes # This file contains data models, view delegates, and associated classes
# for handling the GUI back end data model. # for handling the GUI back end data model.
from PyQt4 import QtCore, QtGui, QtSql from PyQt4 import QtCore, QtGui
import air_modes import air_modes
import threading, math, time import threading, math, time
from air_modes.exceptions import *
from gnuradio.gr.pubsub import pubsub
#fades the ICAOs out as their last report gets older, #fades the ICAOs out as their last report gets older,
#and display ident if available, ICAO otherwise #and display ident if available, ICAO otherwise
@@ -38,16 +36,12 @@ class ICAOViewDelegate(QtGui.QStyledItemDelegate):
painter.drawRect(option.rect) painter.drawRect(option.rect)
#if there's an ident available, use it. otherwise print the ICAO #if there's an ident available, use it. otherwise print the ICAO
if index.model().data(index.model().index(index.row(), 8)) != QtCore.QVariant(): if index.model().data(index.model().index(index.row(), 9)) != QtCore.QVariant():
paintstr = index.model().data(index.model().index(index.row(), 8)).toString() paintstr = index.model().data(index.model().index(index.row(), 9)).toString()
else: else:
paintstr = index.model().data(index.model().index(index.row(), 0)).toString() paintstr = index.model().data(index.model().index(index.row(), 0)).toString()
last_report = index.model().data(index.model().index(index.row(), 1)).toDouble()[0]
#FIXME this is kind of heinous, find out how you got int data out of it last time age = (time.time() - last_report)
last_report = time.strptime(str(index.model().data(index.model().index(index.row(), 1)).toString()), "%Y-%m-%d %H:%M:%S")
age = (time.mktime(time.gmtime()) - time.mktime(last_report)) - 3600.*time.daylight
print age
max_age = 60. #age at which it grays out max_age = 60. #age at which it grays out
#minimum alpha is 0x40 (oldest), max is 0xFF (newest) #minimum alpha is 0x40 (oldest), max is 0xFF (newest)
age = min(age, max_age) age = min(age, max_age)
@@ -55,36 +49,150 @@ class ICAOViewDelegate(QtGui.QStyledItemDelegate):
painter.setPen(QtGui.QColor(0, 0, 0, alpha)) painter.setPen(QtGui.QColor(0, 0, 0, alpha))
painter.drawText(option.rect.left()+3, option.rect.top(), option.rect.width(), option.rect.height(), option.displayAlignment, paintstr) painter.drawText(option.rect.left()+3, option.rect.top(), option.rect.width(), option.rect.height(), option.displayAlignment, paintstr)
#class dashboard_sql_model(QtCore.QAbstractTableModel): #the data model used to display dashboard data.
# def __init__(self, parent): class dashboard_data_model(QtCore.QAbstractTableModel):
# QtCore.QAbstractTableModel.__init__(self, parent)
# def update(self, icao):
#TODO must add libqt4-sql, libqt4-sql-sqlite, python-qt4-sql to dependencies
#TODO looks like you're going to have to either integrate this into sql.py (ugh!) or find a way to keep it in sync
#seems like it wants to have control over maintaining data currency
#worst case is you make your own damn SQL query model based on abstracttablemodel.
class dashboard_sql_model(QtSql.QSqlQueryModel):
def __init__(self, parent): def __init__(self, parent):
QtSql.QSqlQueryModel.__init__(self, parent) QtCore.QAbstractTableModel.__init__(self, parent)
self._query = """select tab1.icao, tab1.seen, tab1.lat, tab1.lon, tab1.alt, speed, heading, vertical, ident, type self._data = []
from (select * from (select * from positions order by seen desc) group by icao) tab1 self.lock = threading.Lock()
left join (select * from (select * from vectors order by seen desc) group by icao) tab2 self._colnames = ["icao", "seen", "rssi", "latitude", "longitude", "altitude", "speed", "heading", "vertical", "ident", "type", "range", "bearing"]
on tab1.icao=tab2.icao #custom precision limits for display
left join (select * from (select * from ident)) tab3 self._precisions = [None, None, None, 6, 6, 0, 0, 0, 0, None, None, 2, 0]
on tab1.icao=tab3.icao for field in self._colnames:
where tab1.seen > datetime('now', '-1 minute')""" self.setHeaderData(self._colnames.index(field), QtCore.Qt.Horizontal, field)
self._sql = None def rowCount(self, parent=QtCore.QVariant()):
self._db = QtSql.QSqlDatabase("QSQLITE") return len(self._data)
self._db.setDatabaseName("adsb.db") #TODO specify this elsewhere def columnCount(self, parent=QtCore.QVariant()):
self._db.open() return len(self._colnames)
#what is this i don't even def data(self, index, role=QtCore.Qt.DisplayRole):
#fetches the combined data of all three tables for all ICAOs seen in the last minute. if not index.isValid():
#FIXME PyQt's SQLite gives you different results than the SQLite browser return QtCore.QVariant()
self.setQuery(self._query, self._db) if index.row() >= self.rowCount():
return QtCore.QVariant()
if index.column() >= self.columnCount():
return QtCore.QVariant()
if (role != QtCore.Qt.DisplayRole) and (role != QtCore.Qt.EditRole):
return QtCore.QVariant()
if self._data[index.row()][index.column()] is None:
return QtCore.QVariant()
else:
#if there's a dedicated precision for that column, print it out with the specified precision.
#this only works well if you DON'T have other views/widgets that depend on numeric data coming out.
#i don't like this, but it works for now. unfortunately it seems like Qt doesn't give you a
#good alternative.
if self._precisions[index.column()] is not None:
return QtCore.QVariant("%.*f" % (self._precisions[index.column()], self._data[index.row()][index.column()]))
else:
if self._colnames[index.column()] == "icao":
return QtCore.QVariant("%06x" % self._data[index.row()][index.column()]) #return as hex string
else:
return QtCore.QVariant(self._data[index.row()][index.column()])
#the big club def setData(self, index, value, role=QtCore.Qt.EditRole):
def update_all(self, icao): self.lock.acquire()
self.setQuery(self._query, self._db) if not index.isValid():
#self.dataChanged.emit(self.createIndex(0, 0), self.createIndex(self.rowCount(), self.columnCount())) return False
if index.row() >= self.rowCount():
return False
if index.column >= self.columnCount():
return False
if role != QtCore.Qt.EditRole:
return False
self._data[index.row()][index.column()] = value
self.lock.release()
#addRecord implements an upsert on self._data; that is,
#it updates the row if the ICAO exists, or else it creates a new row.
def addRecord(self, record):
self.lock.acquire()
icaos = [x[0] for x in self._data]
if record["icao"] in icaos:
row = icaos.index(record["icao"])
for column in record:
self._data[row][self._colnames.index(column)] = record[column]
#create index to existing row and tell the model everything's changed in this row
#or inside the for loop, use dataChanged on each changed field (might be better)
self.dataChanged.emit(self.createIndex(row, 0), self.createIndex(row, len(self._colnames)-1))
#only create records for ICAOs with ADS-B reports
elif ("latitude" or "speed" or "ident") in record:
#find new inserted row number
icaos.append(record["icao"])
newrowoffset = sorted(icaos).index(record["icao"])
self.beginInsertRows(QtCore.QModelIndex(), newrowoffset, newrowoffset)
newrecord = [None for x in xrange(len(self._colnames))]
for col in xrange(0, len(self._colnames)):
if self._colnames[col] in record:
newrecord[col] = record[self._colnames[col]]
self._data.append(newrecord)
self._data = sorted(self._data, key = lambda x: x[0]) #sort by icao
self.endInsertRows()
self.lock.release()
self.prune()
#weeds out ICAOs older than 5 minutes
def prune(self):
self.lock.acquire()
for (index,row) in enumerate(self._data):
if time.time() - row[1] >= 60:
self.beginRemoveRows(QtCore.QModelIndex(), index, index)
self._data.pop(index)
self.endRemoveRows()
self.lock.release()
class dashboard_output(air_modes.parse):
def __init__(self, mypos, model):
air_modes.parse.__init__(self, mypos)
self.model = model
def output(self, msg):
[data, ecc, reference, timestamp] = msg.split()
data = air_modes.modes_reply(long(data, 16))
ecc = long(ecc, 16)
rssi = 10.*math.log10(float(reference))
msgtype = data["df"]
now = time.time()
newrow = {"rssi": rssi, "seen": now}
if msgtype in [0, 4, 20]:
newrow["altitude"] = air_modes.altitude.decode_alt(data["ac"], True)
newrow["icao"] = ecc
self.model.addRecord(newrow)
elif msgtype == 17:
icao = data["aa"]
newrow["icao"] = icao
subtype = data["ftc"]
if subtype == 4:
(ident, actype) = self.parseBDS08(data)
newrow["ident"] = ident
newrow["type"] = actype
elif 5 <= subtype <= 8:
(ground_track, decoded_lat, decoded_lon, rnge, bearing) = self.parseBDS06(data)
newrow["heading"] = ground_track
newrow["latitude"] = decoded_lat
newrow["longitude"] = decoded_lon
newrow["altitude"] = 0
if rnge is not None:
newrow["range"] = rnge
newrow["bearing"] = bearing
elif 9 <= subtype <= 18:
(altitude, decoded_lat, decoded_lon, rnge, bearing) = self.parseBDS05(data)
newrow["altitude"] = altitude
newrow["latitude"] = decoded_lat
newrow["longitude"] = decoded_lon
if rnge is not None:
newrow["range"] = rnge
newrow["bearing"] = bearing
elif subtype == 19:
subsubtype = data["sub"]
velocity = None
heading = None
vert_spd = None
if subsubtype == 0:
(velocity, heading, vert_spd) = self.parseBDS09_0(data)
elif 1 <= subsubtype <= 2:
(velocity, heading, vert_spd) = self.parseBDS09_1(data)
newrow["speed"] = velocity
newrow["heading"] = heading
newrow["vertical"] = vert_spd
self.model.addRecord(newrow)

View File

@@ -1,127 +0,0 @@
#!/usr/bin/env python
#HTML template for Mode S map display
#Nick Foster, 2013
def html_template(my_position, json_file):
if my_position is None:
my_position = [37, -122]
return """
<html>
<head>
<title>ADS-B Aircraft Map</title>
<meta name="viewport" content="initial-scale=1.0, user-scalable=no" />
<meta http-equiv="content-type" content="text/html;charset=utf-8" />
<style type="text/css">
.labels {
color: red;
background-color: white;
font-family: "Lucida Grande", "Arial", sans-serif;
font-size: 10px;
font-weight: bold;
text-align: center;
width: 40px;
border: 2px solid black;
white-space: nowrap;
}
</style>
<script type="text/javascript" src="http://maps.google.com/maps/api/js?sensor=false">
</script>
<script type="text/javascript">
var map;
var markers = [];
var defaultLocation = new google.maps.LatLng(%f, %f);
var defaultZoomLevel = 9;
function requestJSONP() {
var script = document.createElement("script");
script.src = "%s?" + Math.random();
script.params = Math.random();
document.getElementsByTagName('head')[0].appendChild(script);
};
var planeMarker;
var planes = [];
function clearMarkers() {
for (var i = 0; i < planes.length; i++) {
planes[i].setMap(null);
}
planes = [];
};
function jsonp_callback(results) { // from JSONP
clearMarkers();
airplanes = {};
for (var i = 0; i < results.length; i++) {
airplanes[results[i].icao] = {
center: new google.maps.LatLng(results[i].lat, results[i].lon),
heading: results[i].hdg,
altitude: results[i].alt,
type: results[i].type,
ident: results[i].ident,
speed: results[i].speed,
vertical: results[i].vertical,
highlight: results[i].highlight
};
}
refreshIcons();
}
function refreshIcons() {
for (var airplane in airplanes) {
if (airplanes[airplane].highlight != 0) {
icon_file = "http://www.nerdnetworks.org/~bistromath/airplane_sprite_highlight.png";
} else {
icon_file = "http://www.nerdnetworks.org/~bistromath/airplane_sprite.png";
};
var plane_icon = {
url: icon_file,
size: new google.maps.Size(128,128),
origin: new google.maps.Point(parseInt(airplanes[airplane].heading/10)*128,0),
anchor: new google.maps.Point(64,64),
//scaledSize: new google.maps.Size(4608,126)
};
identstr = airplanes[airplane].ident;
if (identstr === "" || !identstr) {
identstr = airplanes[airplane].icao;
};
var planeOptions = {
map: map,
position: airplanes[airplane].center,
icon: plane_icon,
//label content meaningless unless you use the MarkerWithLabel class from the Maps Utility Library
labelContent: identstr,
labelAnchor: new google.maps.Point(64, 0),
labelClass: "labels",
labelStyle: {opacity: 0.75}
};
planeMarker = new google.maps.Marker(planeOptions);
planes.push(planeMarker);
};
};
function initialize()
{
var myOptions =
{
zoom: defaultZoomLevel,
center: defaultLocation,
disableDefaultUI: true,
mapTypeId: google.maps.MapTypeId.TERRAIN
};
map = new google.maps.Map(document.getElementById("map_canvas"), myOptions);
requestJSONP();
setInterval("requestJSONP()", 1000);
};
</script>
</head>
<body onload="initialize()">
<div id="map_canvas" style="width:100%%; height:100%%">
</div>
</body>
</html>""" % (my_position[0], my_position[1], json_file)

View File

@@ -30,30 +30,21 @@ class output_kml(threading.Thread):
self.my_coords = localpos self.my_coords = localpos
self._timeout = timeout self._timeout = timeout
self._lock = lock self._lock = lock
self.shutdown = threading.Event() self.done = False
self.finished = threading.Event()
self.setDaemon(1) self.setDaemon(1)
self.start() self.start()
def run(self): def run(self):
self._db = sqlite3.connect(self._dbname) #read from the db self._db = sqlite3.connect(self._dbname) #read from the db
while self.shutdown.is_set() is False: while self.done is False:
self.writekml() self.writekml()
time.sleep(self._timeout) time.sleep(self._timeout)
self.done = True
self._db.close() self._db.close()
self._db = None self._db = None
self.finished.set()
def close(self):
self.shutdown.set()
self.finished.wait(0.2)
#there's a bug here where self._timeout is long and close() has
#to wait for the sleep to expire before closing. we just bail
#instead with the 0.2 param above.
def writekml(self): def writekml(self):
kmlstr = self.genkml() kmlstr = self.genkml()
if kmlstr is not None: if kmlstr is not None:
@@ -64,7 +55,7 @@ class output_kml(threading.Thread):
def locked_execute(self, c, query): def locked_execute(self, c, query):
with self._lock: with self._lock:
c.execute(query) c.execute(query)
def draw_circle(self, center, rng): def draw_circle(self, center, rng):
retstr = "" retstr = ""
steps=30 steps=30
@@ -91,7 +82,7 @@ class output_kml(threading.Thread):
retstr = string.lstrip(retstr) retstr = string.lstrip(retstr)
return retstr return retstr
def genkml(self): def genkml(self):
#first let's draw the static content #first let's draw the static content
retstr="""<?xml version="1.0" encoding="UTF-8"?>\n<kml xmlns="http://www.opengis.net/kml/2.2">\n<Document>\n\t<Style id="airplane">\n\t\t<IconStyle>\n\t\t\t<Icon><href>airports.png</href></Icon>\n\t\t</IconStyle>\n\t</Style>\n\t<Style id="rangering">\n\t<LineStyle>\n\t\t<color>9f4f4faf</color>\n\t\t<width>2</width>\n\t</LineStyle>\n\t</Style>\n\t<Style id="track">\n\t<LineStyle>\n\t\t<color>5fff8f8f</color>\n\t\t<width>4</width>\n\t</LineStyle>\n\t</Style>""" retstr="""<?xml version="1.0" encoding="UTF-8"?>\n<kml xmlns="http://www.opengis.net/kml/2.2">\n<Document>\n\t<Style id="airplane">\n\t\t<IconStyle>\n\t\t\t<Icon><href>airports.png</href></Icon>\n\t\t</IconStyle>\n\t</Style>\n\t<Style id="rangering">\n\t<LineStyle>\n\t\t<color>9f4f4faf</color>\n\t\t<width>2</width>\n\t</LineStyle>\n\t</Style>\n\t<Style id="track">\n\t<LineStyle>\n\t\t<color>5fff8f8f</color>\n\t\t<width>4</width>\n\t</LineStyle>\n\t</Style>"""
@@ -101,7 +92,7 @@ class output_kml(threading.Thread):
for rng in [100, 200, 300]: for rng in [100, 200, 300]:
retstr += """\n\t\t<Placemark>\n\t\t\t<name>%inm</name>\n\t\t\t<styleUrl>#rangering</styleUrl>\n\t\t\t<LinearRing>\n\t\t\t\t<coordinates>%s</coordinates>\n\t\t\t</LinearRing>\n\t\t</Placemark>""" % (rng, self.draw_circle(self.my_coords, rng),) retstr += """\n\t\t<Placemark>\n\t\t\t<name>%inm</name>\n\t\t\t<styleUrl>#rangering</styleUrl>\n\t\t\t<LinearRing>\n\t\t\t\t<coordinates>%s</coordinates>\n\t\t\t</LinearRing>\n\t\t</Placemark>""" % (rng, self.draw_circle(self.my_coords, rng),)
retstr += """\t</Folder>\n""" retstr += """\t</Folder>\n"""
retstr += """\t<Folder>\n\t\t<name>Aircraft locations</name>\n\t\t<open>0</open>""" retstr += """\t<Folder>\n\t\t<name>Aircraft locations</name>\n\t\t<open>0</open>"""
#read the database and add KML #read the database and add KML
@@ -124,11 +115,11 @@ class output_kml(threading.Thread):
if lon is None: lon = 0 if lon is None: lon = 0
alt = track[0][2] alt = track[0][2]
if alt is None: alt = 0 if alt is None: alt = 0
metric_alt = alt * 0.3048 #google earth takes meters, the commie bastards metric_alt = alt * 0.3048 #google earth takes meters, the commie bastards
trackstr = "" trackstr = ""
for pos in track: for pos in track:
trackstr += " %f,%f,%f" % (pos[4], pos[3], pos[2]*0.3048) trackstr += " %f,%f,%f" % (pos[4], pos[3], pos[2]*0.3048)
@@ -162,7 +153,7 @@ class output_kml(threading.Thread):
seen = 0 seen = 0
speed = 0 speed = 0
heading = 0 heading = 0
vertical = 0 vertical = 0
#now generate some KML #now generate some KML
retstr+= "\n\t\t<Placemark>\n\t\t\t<name>%s</name>\n\t\t\t<Style><IconStyle><heading>%i</heading></IconStyle></Style>\n\t\t\t<styleUrl>#airplane</styleUrl>\n\t\t\t<description>\n\t\t\t\t<![CDATA[Altitude: %s<br/>Heading: %i<br/>Speed: %i<br/>Vertical speed: %i<br/>ICAO: %x<br/>Last seen: %s]]>\n\t\t\t</description>\n\t\t\t<Point>\n\t\t\t\t<altitudeMode>absolute</altitudeMode>\n\t\t\t\t<extrude>1</extrude>\n\t\t\t\t<coordinates>%s,%s,%i</coordinates>\n\t\t\t</Point>\n\t\t</Placemark>" % (ident, heading, alt, heading, speed, vertical, icao[0], seen, lon, lat, metric_alt, ) retstr+= "\n\t\t<Placemark>\n\t\t\t<name>%s</name>\n\t\t\t<Style><IconStyle><heading>%i</heading></IconStyle></Style>\n\t\t\t<styleUrl>#airplane</styleUrl>\n\t\t\t<description>\n\t\t\t\t<![CDATA[Altitude: %s<br/>Heading: %i<br/>Speed: %i<br/>Vertical speed: %i<br/>ICAO: %x<br/>Last seen: %s]]>\n\t\t\t</description>\n\t\t\t<Point>\n\t\t\t\t<altitudeMode>absolute</altitudeMode>\n\t\t\t\t<extrude>1</extrude>\n\t\t\t\t<coordinates>%s,%s,%i</coordinates>\n\t\t\t</Point>\n\t\t</Placemark>" % (ident, heading, alt, heading, speed, vertical, icao[0], seen, lon, lat, metric_alt, )
@@ -170,79 +161,3 @@ class output_kml(threading.Thread):
retstr+= '\n\t</Folder>\n</Document>\n</kml>' retstr+= '\n\t</Folder>\n</Document>\n</kml>'
return retstr return retstr
#we just inherit from output_kml because we're doing the same thing, only in a different format.
class output_jsonp(output_kml):
def set_highlight(self, icao):
self.highlight = icao
def genkml(self):
retstr="""jsonp_callback(["""
# if self.my_coords is not None:
# retstr += """\n\t<Folder>\n\t\t<name>Range rings</name>\n\t\t<open>0</open>"""
# for rng in [100, 200, 300]:
# retstr += """\n\t\t<Placemark>\n\t\t\t<name>%inm</name>\n\t\t\t<styleUrl>#rangering</styleUrl>\n\t\t\t<LinearRing>\n\t\t\t\t<coordinates>%s</coordinates>\n\t\t\t</LinearRing>\n\t\t</Placemark>""" % (rng, self.draw_circle(self.my_coords, rng),)
# retstr += """\t</Folder>\n"""
# retstr += """\t<Folder>\n\t\t<name>Aircraft locations</name>\n\t\t<open>0</open>"""
#read the database and add KML
q = "select distinct icao from positions where seen > datetime('now', '-5 minute')"
c = self._db.cursor()
self.locked_execute(c, q)
icaolist = c.fetchall()
#now we have a list icaolist of all ICAOs seen in the last 5 minutes
for icao in icaolist:
icao = icao[0]
#now get metadata
q = "select ident, type from ident where icao=%i" % icao
self.locked_execute(c, q)
r = c.fetchall()
if len(r) != 0:
ident = r[0][0]
actype = r[0][1]
else:
ident=""
actype = ""
if ident is None: ident = ""
#get most recent speed/heading/vertical
q = "select seen, speed, heading, vertical from vectors where icao=%i order by seen desc limit 1" % icao
self.locked_execute(c, q)
r = c.fetchall()
if len(r) != 0:
seen = r[0][0]
speed = r[0][1]
heading = r[0][2]
vertical = r[0][3]
else:
seen = 0
speed = 0
heading = 0
vertical = 0
q = "select lat, lon, alt from positions where icao=%i order by seen desc limit 1" % icao
self.locked_execute(c, q)
r = c.fetchall()
if len(r) != 0:
lat = r[0][0]
lon = r[0][1]
alt = r[0][2]
else:
lat = 0
lon = 0
alt = 0
highlight = 0
if hasattr(self, 'highlight'):
if self.highlight == icao:
highlight = 1
#now generate some JSONP
retstr+= """{"icao": "%.6x", "lat": %f, "lon": %f, "alt": %i, "hdg": %i, "speed": %i, "vertical": %i, "ident": "%s", "type": "%s", "highlight": %i},""" % (icao, lat, lon, alt, heading, speed, vertical, ident, actype, highlight)
retstr+= """]);"""
return retstr

View File

@@ -95,28 +95,31 @@ c = 299792458 / 1.0003 #modified for refractive index of air, why not
#we use limit as a goal to stop solving when we get "close enough" (error magnitude in meters for that iteration) #we use limit as a goal to stop solving when we get "close enough" (error magnitude in meters for that iteration)
#basically 20 meters is way less than the anticipated error of the system so it doesn't make sense to continue #basically 20 meters is way less than the anticipated error of the system so it doesn't make sense to continue
#it's possible this could fail in situations where the solution converges slowly #it's possible this could fail in situations where the solution converges slowly
#TODO: this fails to converge for some seriously advantageous geometry #THIS WORKS PLEASE DON'T MESS WITH IT
def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 100): def mlat_iter(stations, prange_obs, guess = [0,0,0], limit = 20, maxrounds = 100):
xerr = [1e9, 1e9, 1e9] xerr = [1e9, 1e9, 1e9, 1e9]
rounds = 0 rounds = 0
while numpy.linalg.norm(xerr) > limit: while numpy.linalg.norm(xerr[:3]) > limit:
prange_est = [[numpy.linalg.norm(station - xguess)] for station in rel_stations] #get p_i, the estimated pseudoranges based on the latest position guess
prange_est = [[numpy.linalg.norm(station - guess)] for station in stations]
#get the difference d_p^ between the observed and calculated pseudoranges
dphat = prange_obs - prange_est dphat = prange_obs - prange_est
H = numpy.array([(numpy.array(-rel_stations[row,:])+xguess) / prange_est[row] for row in range(0,len(rel_stations))]) #create a matrix of partial differentials to find the slope of the error in X,Y,Z,t directions
H = numpy.array([(numpy.array(-stations[row,:])+guess) / prange_est[row] for row in range(len(stations))])
H = numpy.append(H, numpy.ones(len(prange_obs)).reshape(len(prange_obs),1)*c, axis=1)
#now we have H, the Jacobian, and can solve for residual error #now we have H, the Jacobian, and can solve for residual error
xerr = numpy.linalg.lstsq(H, dphat)[0].flatten() solved = numpy.linalg.lstsq(H, dphat)
xguess += xerr xerr = solved[0].flatten()
#print xguess, xerr guess += xerr[:3] #we ignore the time error for xguess
rounds += 1 rounds += 1
if rounds > maxrounds: if rounds > maxrounds:
raise Exception("Failed to converge!") raise MlatNonConvergeError("Failed to converge!")
break return (guess, xerr[3])
return xguess
#func mlat: #func mlat:
#uses a modified GPS pseudorange solver to locate aircraft by multilateration. #uses a modified GPS pseudorange solver to locate aircraft by multilateration.
#replies is a list of reports, in ([lat, lon, alt], timestamp) format #replies is a list of reports, in ([lat, lon, alt], timestamp) format
#altitude is the barometric altitude of the aircraft as returned by the aircraft #altitude is the barometric altitude of the aircraft as returned by the aircraft, if any
#returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84. #returns the estimated position of the aircraft in (lat, lon, alt) geoid-corrected WGS84.
#let's make it take a list of tuples so we can sort by them #let's make it take a list of tuples so we can sort by them
def mlat(replies, altitude): def mlat(replies, altitude):
@@ -124,76 +127,70 @@ def mlat(replies, altitude):
stations = [sorted_reply[0] for sorted_reply in sorted_replies] stations = [sorted_reply[0] for sorted_reply in sorted_replies]
timestamps = [sorted_reply[1] for sorted_reply in sorted_replies] timestamps = [sorted_reply[1] for sorted_reply in sorted_replies]
nearest_llh = stations[0]
stations_xyz = numpy.array([llh2geoid(station) for station in stations])
me_llh = stations[0] #the absolute time shouldn't matter at all except perhaps in
me = llh2geoid(stations[0]) #maintaining floating-point precision; in other words you can
#add a constant here and it should fall out in the solver as time error
prange_obs = [[c*stamp] for stamp in timestamps]
#if no alt, use a reasonably large number (in meters)
#list of stations in XYZ relative to me #since if all your stations lie in a plane (they basically will),
rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(me) for station in stations[1:]] #there's a reasonable solution at negative altitude as well
rel_stations.append([0,0,0] - numpy.array(me)) if altitude is None:
rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array altitude = 20000
firstguess = numpy.array(llh2ecef((nearest_llh[0], nearest_llh[1], altitude)))
#differentiate the timestamps to get TDOA, multiply by c to get pseudorange
prange_obs = [[c*(stamp-timestamps[0])] for stamp in timestamps[1:]]
#so here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid
#in other words, we say "if the aircraft were directly overhead of station[0], this is the prange to the center of the earth"
#this is a necessary approximation since we don't know the location of the aircraft yet
#if the dang earth were actually round this wouldn't be an issue
prange_obs.append( [numpy.linalg.norm(llh2ecef((me_llh[0], me_llh[1], altitude)))] ) #use ECEF not geoid since alt is MSL not GPS
prange_obs = numpy.array(prange_obs) prange_obs = numpy.array(prange_obs)
xyzpos, time_offset = mlat_iter(stations_xyz, prange_obs, firstguess, maxrounds=100)
#xguess = llh2ecef([37.617175,-122.400843, 8000])-numpy.array(me) llhpos = ecef2llh(xyzpos)
#xguess = [0,0,0]
#start our guess directly overhead, who cares
xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
xyzpos = mlat_iter(rel_stations, prange_obs, xguess) return (llhpos, time_offset)
llhpos = ecef2llh(xyzpos+me)
#now, we could return llhpos right now and be done with it.
#but the assumption we made above, namely that the aircraft is directly above the
#nearest station, results in significant error due to the oblateness of the Earth's geometry.
#so now we solve AGAIN, but this time with the corrected pseudorange of the aircraft altitude
#this might not be really useful in practice but the sim shows >50m errors without it
#and <4cm errors with it, not that we'll get that close in reality but hey let's do it right
prange_obs[-1] = [numpy.linalg.norm(llh2ecef((llhpos[0], llhpos[1], altitude)))]
xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
llhpos = ecef2llh(xyzpos_corr+me)
#and now, what the hell, let's try to get dilution of precision data #tests the mlat_iter algorithm using sample data from Farrell & Barth (p.147)
#avec is the unit vector of relative ranges to the aircraft from each of the stations def farrell_barth_test():
# for i in range(len(avec)): pranges = numpy.array([[22228206.42],
# avec[i] = numpy.array(avec[i]) / numpy.linalg.norm(numpy.array(avec[i])) [24096139.11],
# numpy.append(avec, [[-1],[-1],[-1],[-1]], 1) #must be # of stations [21729070.63],
# doparray = numpy.linalg.inv(avec.T*avec) [21259581.09]])
#the diagonal elements of doparray will be the x, y, z DOPs. svpos = numpy.array([[7766188.44, -21960535.34, 12522838.56],
[-25922679.66, -6629461.28, 31864.37],
return llhpos [-5743774.02, -25828319.92, 1692757.72],
[-2786005.69, -15900725.80, 21302003.49]])
#this is the "correct" resolved position, not the real receiver position
known_pos = numpy.array( [-2430745.0959362, -4702345.11359277, 3546568.70599656])
pos, time_offset = mlat_iter(svpos, pranges)
print "Position: ", pos
print "LLH: ", ecef2llh(pos)
error = numpy.linalg.norm(pos - known_pos)
print "Error: ", error
if error < 1e-3:
print "Farrell & Barth test OK"
else:
raise Exception("ERROR: Failed Farrell & Barth test")
if __name__ == '__main__': if __name__ == '__main__':
#here's some test data to validate the algorithm #check to see that you haven't screwed up mlat_iter
farrell_barth_test()
#construct simulated returns from these stations
teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]] teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]]
testalt = 8000 testalt = 8000
testplane = numpy.array(llh2ecef([37.617175,-122.400843, testalt])) testplane = numpy.array(llh2ecef([37.617175,-122.400843, testalt]))
testme = llh2geoid(teststations[0]) tx_time = 10 #time offset to apply to timestamps
teststamps = [10, teststamps = [tx_time+numpy.linalg.norm(testplane-numpy.array(llh2geoid(station))) / c for station in teststations]
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[1]))) / c,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[2]))) / c,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[3]))) / c,
]
print teststamps print "Actual pranges: ", sorted([numpy.linalg.norm(testplane - numpy.array(llh2geoid(station))) for station in teststations])
replies = [] replies = [(station, stamp) for station,stamp in zip(teststations, teststamps)]
for i in range(0, len(teststations)):
replies.append((teststations[i], teststamps[i])) ans, offset = mlat(replies, testalt)
ans = mlat(replies, testalt)
error = numpy.linalg.norm(numpy.array(llh2ecef(ans))-numpy.array(testplane)) error = numpy.linalg.norm(numpy.array(llh2ecef(ans))-numpy.array(testplane))
range = numpy.linalg.norm(llh2geoid(ans)-numpy.array(testme)) rng = numpy.linalg.norm(llh2geoid(ans)-numpy.array(llh2geoid(teststations[0])))
print testplane-testme print "Resolved lat/lon/alt: ", ans
print ans
print "Error: %.2fm" % (error) print "Error: %.2fm" % (error)
print "Range: %.2fkm (from first station in list)" % (range/1000) print "Range: %.2fkm (from first station in list)" % (rng/1000)
print "Aircraft-local transmit time: %.8fs" % (offset)

View File

@@ -25,194 +25,198 @@ import air_modes
from air_modes.exceptions import * from air_modes.exceptions import *
import math import math
#TODO get rid of class and convert to functions class output_print(air_modes.parse):
#no need for class here def __init__(self, mypos):
class output_print: air_modes.parse.__init__(self, mypos)
def __init__(self, cpr, publisher, callback=None):
self._cpr = cpr
self._callback = callback
#sub to every function that starts with "handle"
self._fns = [int(l[6:]) for l in dir(self) if l.startswith("handle")]
for i in self._fns:
publisher.subscribe("type%i_dl" % i, getattr(self, "handle%i" % i))
publisher.subscribe("modes_dl", self.catch_nohandler)
@staticmethod
def prefix(msg):
return "(%i %.8f) " % (msg.rssi, msg.timestamp)
def _print(self, msg):
if self._callback is None:
print msg
else:
self._callback(msg)
def catch_nohandler(self, msg):
if msg.data.get_type() not in self._fns:
retstr = output_print.prefix(msg)
retstr += "No handler for message type %i" % msg.data.get_type()
if "aa" not in msg.data.fields:
retstr += " from %.6x" % msg.ecc
else:
retstr += " from %.6x" % msg.data["aa"]
self._print(retstr)
def handle0(self, msg): def parse(self, msg):
if msg.reference == 0.0:
refdb = -150.0
else:
refdb = 20.0*math.log10(msg.reference)
output = "(%.0f %.10f) " % (refdb, float(msg.timestamp));
try: try:
retstr = output_print.prefix(msg) msgtype = msg.data["df"]
retstr += "Type 0 (short A-A surveillance) from %x at %ift" % (msg.ecc, air_modes.decode_alt(msg.data["ac"], True)) if msgtype == 0:
ri = msg.data["ri"] output += self.print0(msg.data, msg.ecc)
if ri == 0: elif msgtype == 4:
retstr += " (No TCAS)" output += self.print4(msg.data, msg.ecc)
elif ri == 2: elif msgtype == 5:
retstr += " (TCAS resolution inhibited)" output += self.print5(msg.data, msg.ecc)
elif ri == 3: elif msgtype == 11:
retstr += " (Vertical TCAS resolution only)" output += self.print11(msg.data, msg.ecc)
elif ri == 4: elif msgtype == 17:
retstr += " (Full TCAS resolution)" output += self.print17(msg.data)
elif ri == 9: elif msgtype == 20 or msgtype == 21 or msgtype == 16:
retstr += " (speed <75kt)" output += self.printTCAS(msg.data, msg.ecc)
elif ri > 9:
retstr += " (speed %i-%ikt)" % (75 * (1 << (ri-10)), 75 * (1 << (ri-9)))
else: else:
raise ADSBError output += "No handler for message type %i from %x (but it's in modes_parse)" % (msgtype, msg.ecc)
return output
except NoHandlerError as e:
output += "No handler for message type %s from %x" % (e.msgtype, msg.ecc)
return output
except MetricAltError:
pass
except CPRNoPositionError:
pass
except ADSBError: def output(self, msg):
return parsed = self.parse(msg)
if parsed is not None:
print self.parse(msg)
if msg.data["vs"] is 1: def print0(self, shortdata, ecc):
[vs, cc, sl, ri, altitude] = self.parse0(shortdata)
retstr = "Type 0 (short A-A surveillance) from %x at %ift" % (ecc, altitude)
if ri == 0:
retstr += " (No TCAS)"
elif ri == 2:
retstr += " (TCAS resolution inhibited)"
elif ri == 3:
retstr += " (Vertical TCAS resolution only)"
elif ri == 4:
retstr += " (Full TCAS resolution)"
elif ri == 9:
retstr += " (speed <75kt)"
elif ri > 9:
retstr += " (speed %i-%ikt)" % (75 * (1 << (ri-10)), 75 * (1 << (ri-9)))
if vs is True:
retstr += " (aircraft is on the ground)" retstr += " (aircraft is on the ground)"
self._print(retstr) return retstr
def print4(self, shortdata, ecc):
[fs, dr, um, altitude] = self.parse4(shortdata)
retstr = "Type 4 (short surveillance altitude reply) from %x at %ift" % (ecc, altitude)
@staticmethod
def fs_text(fs):
if fs == 1: if fs == 1:
return " (aircraft is on the ground)" retstr += " (aircraft is on the ground)"
elif fs == 2: elif fs == 2:
return " (AIRBORNE ALERT)" retstr += " (AIRBORNE ALERT)"
elif fs == 3: elif fs == 3:
return " (GROUND ALERT)" retstr += " (GROUND ALERT)"
elif fs == 4: elif fs == 4:
return " (SPI ALERT)" retstr += " (SPI ALERT)"
elif fs == 5: elif fs == 5:
return " (SPI)" retstr += " (SPI)"
return retstr
def print5(self, shortdata, ecc):
[fs, dr, um, ident] = self.parse5(shortdata)
retstr = "Type 5 (short surveillance ident reply) from %x with ident %i" % (ecc, ident)
if fs == 1:
retstr += " (aircraft is on the ground)"
elif fs == 2:
retstr += " (AIRBORNE ALERT)"
elif fs == 3:
retstr += " (GROUND ALERT)"
elif fs == 4:
retstr += " (SPI ALERT)"
elif fs == 5:
retstr += " (SPI)"
return retstr
def print11(self, data, ecc):
[icao24, interrogator, ca] = self.parse11(data, ecc)
retstr = "Type 11 (all call reply) from %x in reply to interrogator %i with capability level %i" % (icao24, interrogator, ca+1)
return retstr
def print17(self, data):
icao24 = data["aa"]
bdsreg = data["me"].get_type()
retstr = None
if bdsreg == 0x08:
(msg, typestring) = self.parseBDS08(data)
retstr = "Type 17 BDS0,8 (ident) from %x type %s ident %s" % (icao24, typestring, msg)
elif bdsreg == 0x06:
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(data)
retstr = "Type 17 BDS0,6 (surface report) from %x at (%.6f, %.6f) ground track %i" % (icao24, decoded_lat, decoded_lon, ground_track)
if rnge is not None and bearing is not None:
retstr += " (%.2f @ %.0f)" % (rnge, bearing)
elif bdsreg == 0x05:
[altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(data)
retstr = "Type 17 BDS0,5 (position report) from %x at (%.6f, %.6f)" % (icao24, decoded_lat, decoded_lon)
if rnge is not None and bearing is not None:
retstr += " (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")"
retstr += " at " + str(altitude) + "ft"
elif bdsreg == 0x09:
subtype = data["bds09"].get_type()
if subtype == 0:
[velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(data)
retstr = "Type 17 BDS0,9-%i (track report) from %x with velocity %.0fkt heading %.0f VS %.0f turn rate %.0f" \
% (subtype, icao24, velocity, heading, vert_spd, turnrate)
elif subtype == 1:
[velocity, heading, vert_spd] = self.parseBDS09_1(data)
retstr = "Type 17 BDS0,9-%i (track report) from %x with velocity %.0fkt heading %.0f VS %.0f" % (subtype, icao24, velocity, heading, vert_spd)
elif subtype == 3:
[mag_hdg, vel_src, vel, vert_spd, geo_diff] = self.parseBDS09_3(data)
retstr = "Type 17 BDS0,9-%i (air course report) from %x with %s %.0fkt magnetic heading %.0f VS %.0f geo. diff. from baro. alt. %.0fft" \
% (subtype, icao24, vel_src, vel, mag_hdg, vert_spd, geo_diff)
else:
retstr = "Type 17 BDS0,9-%i from %x not implemented" % (subtype, icao24)
elif bdsreg == 0x62:
emerg_str = self.parseBDS62(data)
retstr = "Type 17 BDS6,2 (emergency) from %x type %s" % (icao24, emerg_str)
else: else:
raise ADSBError retstr = "Type 17 with FTC=%i from %x not implemented" % (data["ftc"], icao24)
def handle4(self, msg): return retstr
try:
retstr = output_print.prefix(msg)
retstr += "Type 4 (short surveillance altitude reply) from %x at %ift" % (msg.ecc, air_modes.decode_alt(msg.data["ac"], True))
retstr += output_print.fs_text(msg.data["fs"])
except ADSBError:
return
self._print(retstr)
def handle5(self, msg): def printTCAS(self, data, ecc):
try: msgtype = data["df"]
retstr = output_print.prefix(msg) if msgtype == 20 or msgtype == 16:
retstr += "Type 5 (short surveillance ident reply) from %x with ident %i" % (msg.ecc, air_modes.decode_id(msg.data["id"])) #type 16 does not have fs, dr, um but we get alt here
retstr += output_print.fs_text(msg.data["fs"]) [fs, dr, um, alt] = self.parse4(data)
except ADSBError: elif msgtype == 21:
return [fs, dr, um, ident] = self.parse5(data)
self._print(retstr)
def handle11(self, msg):
try:
retstr = output_print.prefix(msg)
retstr += "Type 11 (all call reply) from %x in reply to interrogator %i with capability level %i" % (msg.data["aa"], msg.ecc & 0xF, msg.data["ca"]+1)
except ADSBError:
return
self._print(retstr)
#the only one which requires state
def handle17(self, msg):
icao24 = msg.data["aa"]
bdsreg = msg.data["me"].get_type()
retstr = output_print.prefix(msg)
try:
if bdsreg == 0x08:
(ident, typestring) = air_modes.parseBDS08(msg.data)
retstr += "Type 17 BDS0,8 (ident) from %x type %s ident %s" % (icao24, typestring, ident)
elif bdsreg == 0x06:
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS06(msg.data, self._cpr)
retstr += "Type 17 BDS0,6 (surface report) from %x at (%.6f, %.6f) ground track %i" % (icao24, decoded_lat, decoded_lon, ground_track)
if rnge is not None and bearing is not None:
retstr += " (%.2f @ %.0f)" % (rnge, bearing)
elif bdsreg == 0x05:
[altitude, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS05(msg.data, self._cpr)
retstr += "Type 17 BDS0,5 (position report) from %x at (%.6f, %.6f)" % (icao24, decoded_lat, decoded_lon)
if rnge is not None and bearing is not None:
retstr += " (" + "%.2f" % rnge + " @ " + "%.0f" % bearing + ")"
retstr += " at " + str(altitude) + "ft"
elif bdsreg == 0x09:
subtype = msg.data["bds09"].get_type()
if subtype == 0:
[velocity, heading, vert_spd, turnrate] = air_modes.parseBDS09_0(msg.data)
retstr += "Type 17 BDS0,9-%i (track report) from %x with velocity %.0fkt heading %.0f VS %.0f turn rate %.0f" \
% (subtype, icao24, velocity, heading, vert_spd, turnrate)
elif subtype == 1:
[velocity, heading, vert_spd] = air_modes.parseBDS09_1(msg.data)
retstr += "Type 17 BDS0,9-%i (track report) from %x with velocity %.0fkt heading %.0f VS %.0f" % (subtype, icao24, velocity, heading, vert_spd)
elif subtype == 3:
[mag_hdg, vel_src, vel, vert_spd, geo_diff] = air_modes.parseBDS09_3(msg.data)
retstr += "Type 17 BDS0,9-%i (air course report) from %x with %s %.0fkt magnetic heading %.0f VS %.0f geo. diff. from baro. alt. %.0fft" \
% (subtype, icao24, vel_src, vel, mag_hdg, vert_spd, geo_diff)
else:
retstr += "Type 17 BDS0,9-%i from %x not implemented" % (subtype, icao24)
elif bdsreg == 0x62:
emerg_str = air_modes.parseBDS62(data)
retstr += "Type 17 BDS6,2 (emergency) from %x type %s" % (icao24, emerg_str)
else:
retstr += "Type 17 with FTC=%i from %x not implemented" % (msg.data["ftc"], icao24)
except ADSBError:
return
self._print(retstr)
def printTCAS(self, msg):
msgtype = msg.data["df"]
if msgtype == 16: if msgtype == 16:
bds1 = msg.data["vds1"] bds1 = data["vds1"]
bds2 = msg.data["vds2"] bds2 = data["vds2"]
else: else:
bds1 = msg.data["bds1"] bds1 = data["bds1"]
bds2 = msg.data["bds2"] bds2 = data["bds2"]
retstr = output_print.prefix(msg)
if bds2 != 0: if bds2 != 0:
retstr += "No handler in type %i for BDS2 == %i from %x" % (msgtype, bds2, msg.ecc) retstr = "No handler in type %i for BDS2 == %i from %x" % (msgtype, bds2, ecc)
elif bds1 == 0: elif bds1 == 0:
retstr += "No handler in type %i for BDS1 == 0 from %x" % (msgtype, msg.ecc) retstr = "No handler in type %i for BDS1 == 0 from %x" % (msgtype, ecc)
elif bds1 == 1: elif bds1 == 1:
retstr += "Type %i link capability report from %x: ACS: 0x%x, BCS: 0x%x, ECS: 0x%x, continues %i" \ retstr = "Type %i link capability report from %x: ACS: 0x%x, BCS: 0x%x, ECS: 0x%x, continues %i" \
% (msgtype, msg.ecc, msg.data["acs"], msg.data["bcs"], msg.data["ecs"], msg.data["cfs"]) % (msgtype, ecc, data["acs"], data["bcs"], data["ecs"], data["cfs"])
elif bds1 == 2: elif bds1 == 2:
retstr += "Type %i identification from %x with text %s" % (msgtype, msg.ecc, air_modes.parseMB_id(msg.data)) retstr = "Type %i identification from %x with text %s" % (msgtype, ecc, self.parseMB_id(data))
elif bds1 == 3: elif bds1 == 3:
retstr += "Type %i TCAS report from %x: " % (msgtype, msg.ecc) retstr = "Type %i TCAS report from %x: " % (msgtype, ecc)
tti = msg.data["tti"] tti = data["tti"]
if msgtype == 16: if msgtype == 16:
(resolutions, complements, rat, mte) = air_modes.parse_TCAS_CRM(msg.data) (resolutions, complements, rat, mte) = self.parse_TCAS_CRM(data)
retstr += "advised: %s complement: %s" % (resolutions, complements) retstr += "advised: %s complement: %s" % (resolutions, complements)
else: else:
if tti == 1: if tti == 1:
(resolutions, complements, rat, mte, threat_id) = air_modes.parseMB_TCAS_threatid(msg.data) (resolutions, complements, rat, mte, threat_id) = self.parseMB_TCAS_threatid(data)
retstr += "threat ID: %x advised: %s complement: %s" % (threat_id, resolutions, complements) retstr += "threat ID: %x advised: %s complement: %s" % (threat_id, resolutions, complements)
elif tti == 2: elif tti == 2:
(resolutions, complements, rat, mte, threat_alt, threat_range, threat_bearing) = air_modes.parseMB_TCAS_threatloc(msg.data) (resolutions, complements, rat, mte, threat_alt, threat_range, threat_bearing) = self.parseMB_TCAS_threatloc(data)
retstr += "range: %i bearing: %i alt: %i advised: %s complement: %s" % (threat_range, threat_bearing, threat_alt, resolutions, complements) retstr += "range: %i bearing: %i alt: %i advised: %s complement: %s" % (threat_range, threat_bearing, threat_alt, resolutions, complements)
else: else:
rat = 0 rat = 0
@@ -223,15 +227,11 @@ class output_print:
if rat == 1: if rat == 1:
retstr += " (resolved)" retstr += " (resolved)"
else: else:
retstr += "No handler for type %i, BDS1 == %i from %x" % (msgtype, bds1, msg.ecc) retstr = "No handler for BDS1 == %i from %x" % (bds1, ecc)
if(msgtype == 20 or msgtype == 16): if(msgtype == 20 or msgtype == 16):
retstr += " at %ift" % air_modes.decode_alt(msg.data["ac"], True) retstr += " at %ift" % alt
else: else:
retstr += " ident %x" % air_modes.decode_id(msg.data["id"]) retstr += " ident %x" % ident
self._print(retstr) return retstr
handle16 = printTCAS
handle20 = printTCAS
handle21 = printTCAS

View File

@@ -23,8 +23,8 @@ import time, os, sys
from string import split, join from string import split, join
from altitude import decode_alt from altitude import decode_alt
import math import math
import air_modes
from air_modes.exceptions import * from air_modes.exceptions import *
from air_modes import cpr
#this implements a packet class which can retrieve its own fields. #this implements a packet class which can retrieve its own fields.
class data_field: class data_field:
@@ -231,207 +231,198 @@ class modes_reply(data_field):
def get_type(self): def get_type(self):
return self.get_bits(1,5) return self.get_bits(1,5)
#unscramble mode A/C-style squawk codes for type 5 replies below class parse:
def decode_id(id): def __init__(self, mypos):
self.my_location = mypos
C1 = 0x1000 self.cpr = cpr.cpr_decoder(self.my_location)
A1 = 0x0800
C2 = 0x0400 def parse0(self, data):
A2 = 0x0200 #this represents the order in which the bits come altitude = decode_alt(data["ac"], True)
C4 = 0x0100 return [data["vs"], data["cc"], data["sl"], data["ri"], altitude]
A4 = 0x0080
B1 = 0x0020
D1 = 0x0010
B2 = 0x0008
D2 = 0x0004
B4 = 0x0002
D4 = 0x0001
a = ((id & A1) >> 11) + ((id & A2) >> 8) + ((id & A4) >> 5)
b = ((id & B1) >> 5) + ((id & B2) >> 2) + ((id & B4) << 1)
c = ((id & C1) >> 12) + ((id & C2) >> 9) + ((id & C4) >> 6)
d = ((id & D1) >> 2) + ((id & D2) >> 1) + ((id & D4) << 2)
return (a * 1000) + (b * 100) + (c * 10) + d
#decode ident squawks def parse4(self, data):
def charmap(d): altitude = decode_alt(data["ac"], True)
if d > 0 and d < 27: return [data["fs"], data["dr"], data["um"], altitude]
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 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"]]
def parseBDS08(data):
categories = [["NO INFO", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED"],\ categories = [["NO INFO", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED", "RESERVED"],\
["NO INFO", "SURFACE EMERGENCY VEHICLE", "SURFACE SERVICE VEHICLE", "FIXED OBSTRUCTION", "CLUSTER OBSTRUCTION", "LINE OBSTRUCTION", "RESERVED"],\ ["NO INFO", "SURFACE EMERGENCY VEHICLE", "SURFACE SERVICE VEHICLE", "FIXED OBSTRUCTION", "CLUSTER OBSTRUCTION", "LINE OBSTRUCTION", "RESERVED"],\
["NO INFO", "GLIDER", "BALLOON/BLIMP", "PARACHUTE", "ULTRALIGHT", "RESERVED", "UAV", "SPACECRAFT"],\ ["NO INFO", "GLIDER", "BALLOON/BLIMP", "PARACHUTE", "ULTRALIGHT", "RESERVED", "UAV", "SPACECRAFT"],\
["NO INFO", "LIGHT", "SMALL", "LARGE", "LARGE HIGH VORTEX", "HEAVY", "HIGH PERFORMANCE", "ROTORCRAFT"]] ["NO INFO", "LIGHT", "SMALL", "LARGE", "LARGE HIGH VORTEX", "HEAVY", "HIGH PERFORMANCE", "ROTORCRAFT"]]
catstring = categories[data["ftc"]-1][data["cat"]] def parseBDS08(self, data):
catstring = self.categories[data["ftc"]-1][data["cat"]]
msg = "" msg = ""
for i in range(0, 8): for i in range(0, 8):
msg += charmap(data["ident"] >> (42-6*i) & 0x3F) msg += self.charmap(data["ident"] >> (42-6*i) & 0x3F)
return (msg, catstring) return (msg, catstring)
#NOTE: this is stateful -- requires CPR decoder def charmap(self, d):
def parseBDS05(data, cprdec): if d > 0 and d < 27:
altitude = decode_alt(data["alt"], False) retval = chr(ord("A")+d-1)
[decoded_lat, decoded_lon, rnge, bearing] = cprdec.decode(data["aa"], data["lat"], data["lon"], data["cpr"], 0) elif d == 32:
return [altitude, decoded_lat, decoded_lon, rnge, bearing] retval = " "
elif d > 47 and d < 58:
retval = chr(ord("0")+d-48)
else:
retval = " "
#NOTE: this is stateful -- requires CPR decoder return retval
def parseBDS06(data, cprdec):
ground_track = data["gtk"] * 360. / 128
[decoded_lat, decoded_lon, rnge, bearing] = cprdec.decode(data["aa"], data["lat"], data["lon"], data["cpr"], 1)
return [ground_track, decoded_lat, decoded_lon, rnge, bearing]
def parseBDS09_0(data): def parseBDS05(self, data):
#0: ["sub", "dew", "vew", "dns", "vns", "str", "tr", "svr", "vr"], icao24 = data["aa"]
vert_spd = data["vr"] * 32
ud = bool(data["dvr"]) encoded_lon = data["lon"]
if ud: encoded_lat = data["lat"]
vert_spd = 0 - vert_spd cpr_format = data["cpr"]
turn_rate = data["tr"] * 15/62 altitude = decode_alt(data["alt"], False)
rl = data["str"]
if rl: [decoded_lat, decoded_lon, rnge, bearing] = self.cpr.decode(icao24, encoded_lat, encoded_lon, cpr_format, 0)
turn_rate = 0 - turn_rate
ns_vel = data["vns"] - 1 return [altitude, decoded_lat, decoded_lon, rnge, bearing]
ns = bool(data["dns"])
ew_vel = data["vew"] - 1
ew = bool(data["dew"]) #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["lon"]
encoded_lat = data["lat"]
cpr_format = data["cpr"]
ground_track = data["gtk"] * 360. / 128
[decoded_lat, decoded_lon, rnge, bearing] = self.cpr.decode(icao24, encoded_lat, encoded_lon, cpr_format, 1)
return [ground_track, decoded_lat, decoded_lon, rnge, bearing]
def parseBDS09_0(self, data):
#0: ["sub", "dew", "vew", "dns", "vns", "str", "tr", "svr", "vr"],
vert_spd = data["vr"] * 32
ud = bool(data["dvr"])
if ud:
vert_spd = 0 - vert_spd
turn_rate = data["tr"] * 15/62
rl = data["str"]
if rl:
turn_rate = 0 - turn_rate
ns_vel = data["vns"] - 1
ns = bool(data["dns"])
ew_vel = data["vew"] - 1
ew = bool(data["dew"])
velocity = math.hypot(ns_vel, ew_vel) velocity = math.hypot(ns_vel, ew_vel)
if ew: if ew:
ew_vel = 0 - ew_vel ew_vel = 0 - ew_vel
if ns: if ns:
ns_vel = 0 - ns_vel ns_vel = 0 - ns_vel
heading = math.atan2(ew_vel, ns_vel) * (180.0 / math.pi) heading = math.atan2(ew_vel, ns_vel) * (180.0 / math.pi)
if heading < 0: if heading < 0:
heading += 360 heading += 360
return [velocity, heading, vert_spd, turn_rate] return [velocity, heading, vert_spd, turn_rate]
def parseBDS09_1(data): def parseBDS09_1(self, data):
#1: ["sub", "icf", "ifr", "nuc", "dew", "vew", "dns", "vns", "vrsrc", "dvr", "vr", "dhd", "hd"], #1: ["sub", "icf", "ifr", "nuc", "dew", "vew", "dns", "vns", "vrsrc", "dvr", "vr", "dhd", "hd"],
alt_geo_diff = data["hd"] * 25 alt_geo_diff = data["hd"] * 25
above_below = bool(data["dhd"]) above_below = bool(data["dhd"])
if above_below: if above_below:
alt_geo_diff = 0 - alt_geo_diff; alt_geo_diff = 0 - alt_geo_diff;
vert_spd = float(data["vr"] - 1) * 64 vert_spd = float(data["vr"] - 1) * 64
ud = bool(data["dvr"]) ud = bool(data["dvr"])
if ud: if ud:
vert_spd = 0 - vert_spd vert_spd = 0 - vert_spd
vert_src = bool(data["vrsrc"]) vert_src = bool(data["vrsrc"])
ns_vel = float(data["vns"]) ns_vel = float(data["vns"])
ns = bool(data["dns"]) ns = bool(data["dns"])
ew_vel = float(data["vew"]) ew_vel = float(data["vew"])
ew = bool(data["dew"]) ew = bool(data["dew"])
subtype = data["sub"] subtype = data["sub"]
if subtype == 0x02: if subtype == 0x02:
ns_vel <<= 2 ns_vel <<= 2
ew_vel <<= 2 ew_vel <<= 2
velocity = math.hypot(ns_vel, ew_vel) velocity = math.hypot(ns_vel, ew_vel)
if ew: if ew:
ew_vel = 0 - ew_vel ew_vel = 0 - ew_vel
if ns_vel == 0: if ns_vel == 0:
heading = 0 heading = 0
else: else:
heading = math.atan(float(ew_vel) / float(ns_vel)) * (180.0 / math.pi) heading = math.atan(float(ew_vel) / float(ns_vel)) * (180.0 / math.pi)
if ns: if ns:
heading = 180 - heading heading = 180 - heading
if heading < 0: if heading < 0:
heading += 360 heading += 360
return [velocity, heading, vert_spd] return [velocity, heading, vert_spd]
def parseBDS09_3(data): def parseBDS09_3(self, data):
#3: {"sub", "icf", "ifr", "nuc", "mhs", "hdg", "ast", "spd", "vrsrc", #3: {"sub", "icf", "ifr", "nuc", "mhs", "hdg", "ast", "spd", "vrsrc",
# "dvr", "vr", "dhd", "hd"} # "dvr", "vr", "dhd", "hd"}
mag_hdg = data["mhs"] * 360. / 1024 mag_hdg = data["mhs"] * 360. / 1024
vel_src = "TAS" if data["ast"] == 1 else "IAS" vel_src = "TAS" if data["ast"] == 1 else "IAS"
vel = data["spd"] vel = data["spd"]
if data["sub"] == 4: if data["sub"] == 4:
vel *= 4 vel *= 4
vert_spd = float(data["vr"] - 1) * 64 vert_spd = float(data["vr"] - 1) * 64
if data["dvr"] == 1: if data["dvr"] == 1:
vert_spd = 0 - vert_spd vert_spd = 0 - vert_spd
geo_diff = float(data["hd"] - 1) * 25 geo_diff = float(data["hd"] - 1) * 25
return [mag_hdg, vel_src, vel, vert_spd, geo_diff] return [mag_hdg, vel_src, vel, vert_spd, geo_diff]
def parseBDS62(data): def parseBDS62(self, data):
eps_strings = ["NO EMERGENCY", "GENERAL EMERGENCY", "LIFEGUARD/MEDICAL", "FUEL EMERGENCY", eps_strings = ["NO EMERGENCY", "GENERAL EMERGENCY", "LIFEGUARD/MEDICAL", "FUEL EMERGENCY",
"NO COMMUNICATIONS", "UNLAWFUL INTERFERENCE", "RESERVED", "RESERVED"] "NO COMMUNICATIONS", "UNLAWFUL INTERFERENCE", "RESERVED", "RESERVED"]
return eps_strings[data["eps"]] return eps_strings[data["eps"]]
def parseMB_id(data): #bds1 == 2, bds2 == 0 def parseMB_id(self, data): #bds1 == 2, bds2 == 0
msg = "" msg = ""
for i in range(0, 8): for i in range(0, 8):
msg += charmap( data["ais"] >> (42-6*i) & 0x3F) msg += self.charmap( data["ais"] >> (42-6*i) & 0x3F)
return (msg) return (msg)
def parseMB_TCAS_resolutions(data): def parseMB_TCAS_resolutions(self, data):
#these are LSB because the ICAO are asshats #these are LSB because the ICAO are asshats
ara_bits = {41: "CLIMB", 42: "DON'T DESCEND", 43: "DON'T DESCEND >500FPM", 44: "DON'T DESCEND >1000FPM", ara_bits = {41: "CLIMB", 42: "DON'T DESCEND", 43: "DON'T DESCEND >500FPM", 44: "DON'T DESCEND >1000FPM",
45: "DON'T DESCEND >2000FPM", 46: "DESCEND", 47: "DON'T CLIMB", 48: "DON'T CLIMB >500FPM", 45: "DON'T DESCEND >2000FPM", 46: "DESCEND", 47: "DON'T CLIMB", 48: "DON'T CLIMB >500FPM",
49: "DON'T CLIMB >1000FPM", 50: "DON'T CLIMB >2000FPM", 51: "TURN LEFT", 52: "TURN RIGHT", 49: "DON'T CLIMB >1000FPM", 50: "DON'T CLIMB >2000FPM", 51: "TURN LEFT", 52: "TURN RIGHT",
53: "DON'T TURN LEFT", 54: "DON'T TURN RIGHT"} 53: "DON'T TURN LEFT", 54: "DON'T TURN RIGHT"}
rac_bits = {55: "DON'T DESCEND", 56: "DON'T CLIMB", 57: "DON'T TURN LEFT", 58: "DON'T TURN RIGHT"} rac_bits = {55: "DON'T DESCEND", 56: "DON'T CLIMB", 57: "DON'T TURN LEFT", 58: "DON'T TURN RIGHT"}
ara = data["ara"] ara = data["ara"]
rac = data["rac"] rac = data["rac"]
#check to see which bits are set #check to see which bits are set
resolutions = "" resolutions = ""
for bit in ara_bits: for bit in ara_bits:
if ara & (1 << (54-bit)): if ara & (1 << (54-bit)):
resolutions += " " + ara_bits[bit] resolutions += " " + ara_bits[bit]
complements = "" complements = ""
for bit in rac_bits: for bit in rac_bits:
if rac & (1 << (58-bit)): if rac & (1 << (58-bit)):
complements += " " + rac_bits[bit] complements += " " + rac_bits[bit]
return (resolutions, complements) return (resolutions, complements)
#rat is 1 if resolution advisory terminated <18s ago #rat is 1 if resolution advisory terminated <18s ago
#mte is 1 if multiple threats indicated #mte is 1 if multiple threats indicated
#tti is threat type: 1 if ID, 2 if range/brg/alt #tti is threat type: 1 if ID, 2 if range/brg/alt
#tida is threat altitude in Mode C format #tida is threat altitude in Mode C format
def parseMB_TCAS_threatid(data): #bds1==3, bds2==0, TTI==1 def parseMB_TCAS_threatid(self, data): #bds1==3, bds2==0, TTI==1
#3: {"bds1": (33,4), "bds2": (37,4), "ara": (41,14), "rac": (55,4), "rat": (59,1), #3: {"bds1": (33,4), "bds2": (37,4), "ara": (41,14), "rac": (55,4), "rat": (59,1),
# "mte": (60,1), "tti": (61,2), "tida": (63,13), "tidr": (76,7), "tidb": (83,6)} # "mte": (60,1), "tti": (61,2), "tida": (63,13), "tidr": (76,7), "tidb": (83,6)}
(resolutions, complements) = parseMB_TCAS_resolutions(data) (resolutions, complements) = self.parseMB_TCAS_resolutions(data)
return (resolutions, complements, data["rat"], data["mte"], data["tid"]) return (resolutions, complements, data["rat"], data["mte"], data["tid"])
def parseMB_TCAS_threatloc(data): #bds1==3, bds2==0, TTI==2 def parseMB_TCAS_threatloc(self, data): #bds1==3, bds2==0, TTI==2
(resolutions, complements) = parseMB_TCAS_resolutions(data) (resolutions, complements) = self.parseMB_TCAS_resolutions(data)
threat_alt = decode_alt(data["tida"], True) threat_alt = decode_alt(data["tida"], True)
return (resolutions, complements, data["rat"], data["mte"], threat_alt, data["tidr"], data["tidb"]) return (resolutions, complements, data["rat"], data["mte"], threat_alt, data["tidr"], data["tidb"])
#type 16 Coordination Reply Message #type 16 Coordination Reply Message
def parse_TCAS_CRM(data): def parse_TCAS_CRM(self, data):
(resolutions, complements) = parseMB_TCAS_resolutions(data) (resolutions, complements) = self.parseMB_TCAS_resolutions(data)
return (resolutions, complements, data["rat"], data["mte"]) return (resolutions, complements, data["rat"], data["mte"])
#this decorator takes a pubsub and returns a function which parses and publishes messages
def make_parser(pub):
publisher = pub
def publish(message):
[data, ecc, reference, timestamp] = message.split()
try:
ret = air_modes.modes_report(modes_reply(int(data, 16)),
int(ecc, 16),
20.0*math.log10(float(reference)),
air_modes.stamp(0, float(timestamp)))
pub["modes_dl"] = ret
pub["type%i_dl" % ret.data.get_type()] = ret
except ADSBError:
pass
return publish

View File

@@ -1,208 +0,0 @@
# Copyright 2013 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.
#
# Radio interface for Mode S RX.
# Handles all hardware- and source-related functionality
# You pass it options, it gives you data.
# It uses the pubsub interface to allow clients to subscribe to its data feeds.
from gnuradio import gr, gru, eng_notation, filter
from gnuradio.filter import optfir
from gnuradio.eng_option import eng_option
from gnuradio.gr.pubsub import pubsub
from optparse import OptionParser, OptionGroup
import air_modes
import zmq
import threading
import time
import re
class modes_radio (gr.top_block, pubsub):
def __init__(self, options, context):
gr.top_block.__init__(self)
pubsub.__init__(self)
self._options = options
self._queue = gr.msg_queue()
self._rate = int(options.rate)
self._resample = None
self._setup_source(options)
self._rx_path = air_modes.rx_path(self._rate, options.threshold, self._queue, options.pmf)
#now subscribe to set various options via pubsub
self.subscribe("freq", self.set_freq)
self.subscribe("gain", self.set_gain)
self.subscribe("rate", self.set_rate)
self.subscribe("rate", self._rx_path.set_rate)
self.subscribe("threshold", self._rx_path.set_threshold)
self.subscribe("pmf", self._rx_path.set_pmf)
self.publish("freq", self.get_freq)
self.publish("gain", self.get_gain)
self.publish("rate", self.get_rate)
self.publish("threshold", self._rx_path.get_threshold)
self.publish("pmf", self._rx_path.get_pmf)
if self._resample is not None:
self.connect(self._u, self._resample, self._rx_path)
else:
self.connect(self._u, self._rx_path)
#Publish messages when they come back off the queue
server_addr = ["inproc://modes-radio-pub"]
if options.tcp is not None:
server_addr += ["tcp://*:%i" % options.tcp]
self._sender = air_modes.zmq_pubsub_iface(context, subaddr=None, pubaddr=server_addr)
self._async_sender = gru.msgq_runner(self._queue, self.send)
def send(self, msg):
self._sender["dl_data"] = msg.to_string()
@staticmethod
def add_radio_options(parser):
group = OptionGroup(parser, "Receiver setup options")
#Choose source
group.add_option("-s","--source", type="string", default="uhd",
help="Choose source: uhd, osmocom, <filename>, or <ip:port> [default=%default]")
group.add_option("-t","--tcp", type="int", default=None, metavar="PORT",
help="Open a TCP server on this port to publish reports")
#UHD/Osmocom args
group.add_option("-R", "--subdev", type="string",
help="select USRP Rx side A or B", metavar="SUBDEV")
group.add_option("-A", "--antenna", type="string",
help="select which antenna to use on daughterboard")
group.add_option("-D", "--args", type="string",
help="arguments to pass to radio constructor", default="")
group.add_option("-f", "--freq", type="eng_float", default=1090e6,
help="set receive frequency in Hz [default=%default]", metavar="FREQ")
group.add_option("-g", "--gain", type="int", default=None,
help="set RF gain", metavar="dB")
#RX path args
group.add_option("-r", "--rate", type="eng_float", default=4e6,
help="set sample rate [default=%default]")
group.add_option("-T", "--threshold", type="eng_float", default=5.0,
help="set pulse detection threshold above noise in dB [default=%default]")
group.add_option("-p","--pmf", action="store_true", default=False,
help="Use pulse matched filtering [default=%default]")
parser.add_option_group(group)
def live_source(self):
return self._options.source is 'uhd' or self._options.source is 'osmocom'
def set_freq(self, freq):
return self._u.set_center_freq(freq, 0) if live_source() else 0
def set_gain(self, gain):
return self._u.set_gain(gain) if live_source() else 0
def set_rate(self, rate):
return self._u.set_rate(rate) if live_source() else 0
def get_freq(self, freq):
return self._u.get_center_freq(freq, 0) if live_source() else 1090e6
def get_gain(self, gain):
return self._u.get_gain() if live_source() else 0
def get_rate(self, rate):
return self._u.get_rate() if live_source() else self._rate
def _setup_source(self, options):
if options.source == "uhd":
#UHD source by default
from gnuradio import uhd
self._u = uhd.single_usrp_source(options.args, uhd.io_type_t.COMPLEX_FLOAT32, 1)
if(options.subdev):
self._u.set_subdev_spec(options.subdev, 0)
if not self._u.set_center_freq(options.freq):
print "Failed to set initial frequency"
#check for GPSDO
#if you have a GPSDO, UHD will automatically set the timestamp to UTC time
#as well as automatically set the clock to lock to GPSDO.
if self._u.get_time_source(0) != 'gpsdo':
self._u.set_time_now(uhd.time_spec(0.0))
if options.antenna is not None:
self._u.set_antenna(options.antenna)
self._u.set_samp_rate(options.rate)
options.rate = int(self._u.get_samp_rate()) #retrieve actual
if options.gain is None: #set to halfway
g = self._u.get_gain_range()
options.gain = (g.start()+g.stop()) / 2.0
print "Setting gain to %i" % options.gain
self._u.set_gain(options.gain)
print "Gain is %i" % self._u.get_gain()
#TODO: detect if you're using an RTLSDR or Jawbreaker
#and set up accordingly.
#ALSO TODO: Actually set gain appropriately using gain bins in HackRF driver.
#osmocom doesn't have gain bucket distribution like UHD does
elif options.source == "osmocom": #RTLSDR dongle or HackRF Jawbreaker
import osmosdr
self._u = osmosdr.source_c(options.args)
# self._u.set_sample_rate(3.2e6) #fixed for RTL dongles
self._u.set_sample_rate(options.rate)
if not self._u.set_center_freq(options.freq):
print "Failed to set initial frequency"
self._u.set_gain_mode(0) #manual gain mode
if options.gain is None:
options.gain = 34
###DO NOT COMMIT
self._u.set_gain(14, "RF", 0)
self._u.set_gain(40, "IF", 0)
#self._u.set_gain(14, "BB", 0)
###DO NOT COMMIT
# self._u.set_gain(options.gain)
print "Gain is %i" % self._u.get_gain()
#Note: this should only come into play if using an RTLSDR.
# lpfiltcoeffs = gr.firdes.low_pass(1, 5*3.2e6, 1.6e6, 300e3)
# self._resample = filter.rational_resampler_ccf(interpolation=5, decimation=4, taps=lpfiltcoeffs)
else:
#semantically detect whether it's ip.ip.ip.ip:port or filename
if ':' in options.source:
try:
ip, port = re.search("(.*)\:(\d{1,5})", options.source).groups()
except:
raise Exception("Please input UDP source e.g. 192.168.10.1:12345")
self._u = gr.udp_source(gr.sizeof_gr_complex, ip, int(port))
print "Using UDP source %s:%s" % (ip, port)
else:
self._u = gr.file_source(gr.sizeof_gr_complex, options.source)
print "Using file source %s" % options.source
print "Rate is %i" % (options.rate,)
def close(self):
self._sender.close()

View File

@@ -1,5 +1,5 @@
# #
# Copyright 2012, 2013 Corgan Labs, Nick Foster # Copyright 2012 Corgan Labs
# #
# This file is part of gr-air-modes # This file is part of gr-air-modes
# #
@@ -19,7 +19,7 @@
# Boston, MA 02110-1301, USA. # Boston, MA 02110-1301, USA.
# #
from gnuradio import gr, blocks from gnuradio import gr
import air_modes_swig import air_modes_swig
class rx_path(gr.hier_block2): class rx_path(gr.hier_block2):
@@ -35,17 +35,17 @@ class rx_path(gr.hier_block2):
self._spc = int(rate/2e6) self._spc = int(rate/2e6)
# Convert incoming I/Q baseband to amplitude # Convert incoming I/Q baseband to amplitude
self._demod = blocks.complex_to_mag() self._demod = gr.complex_to_mag()
self._bb = self._demod self._bb = self._demod
# Pulse matched filter for 0.5us pulses # Pulse matched filter for 0.5us pulses
if use_pmf: if use_pmf:
self._pmf = blocks.moving_average_ff(self._spc, 1.0/self._spc)#, self._rate) self._pmf = gr.moving_average_ff(self._spc, 1.0/self._spc, self._rate)
self.connect(self._demod, self._pmf) self.connect(self._demod, self._pmf)
self._bb = self._pmf self._bb = self._pmf
# Establish baseline amplitude (noise, interference) # Establish baseline amplitude (noise, interference)
self._avg = blocks.moving_average_ff(48*self._spc, 1.0/(48*self._spc))#, self._rate) # 3 preambles self._avg = gr.moving_average_ff(48*self._spc, 1.0/(48*self._spc), self._rate) # 3 preambles
# Synchronize to Mode-S preamble # Synchronize to Mode-S preamble
self._sync = air_modes_swig.modes_preamble(self._rate, self._threshold) self._sync = air_modes_swig.modes_preamble(self._rate, self._threshold)
@@ -58,25 +58,3 @@ class rx_path(gr.hier_block2):
self.connect(self._bb, (self._sync, 0)) self.connect(self._bb, (self._sync, 0))
self.connect(self._bb, self._avg, (self._sync, 1)) self.connect(self._bb, self._avg, (self._sync, 1))
self.connect(self._sync, self._slicer) self.connect(self._sync, self._slicer)
def set_rate(self, rate):
self._sync.set_rate(rate)
self._slicer.set_rate(rate)
self._spc = int(rate/2e6)
self._avg.set_length_and_scale(48*self._spc, 1.0/(48*self._spc))
if self._bb != self._demod:
self._pmf.set_length_and_scale(self._spc, 1.0/self._spc)
def set_threshold(self, threshold):
self._sync.set_threshold(threshold)
def set_pmf(self, pmf):
#TODO must be done when top block is stopped
pass
def get_pmf(self, pmf):
return not (self._bb == self._demod)
def get_threshold(self, threshold):
return self._sync.get_threshold()

View File

@@ -25,30 +25,10 @@ from string import split, join
import air_modes import air_modes
from datetime import * from datetime import *
from air_modes.exceptions import * from air_modes.exceptions import *
import threading
class dumb_task_runner(threading.Thread): class output_sbs1(air_modes.parse):
def __init__(self, task, interval): def __init__(self, mypos, port):
threading.Thread.__init__(self) air_modes.parse.__init__(self, mypos)
self._task = task
self._interval = interval
self.shutdown = threading.Event()
self.finished = threading.Event()
self.setDaemon(True)
self.start()
def run(self):
while not self.shutdown.is_set():
self._task()
time.sleep(self._interval)
self.finished.set()
def close(self):
self.shutdown.set()
self.finished.wait(self._interval)
class output_sbs1:
def __init__(self, cprdec, port, pub):
self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self._s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._s.bind(('', port)) self._s.bind(('', port))
@@ -58,16 +38,6 @@ class output_sbs1:
self._aircraft_id_map = {} # dictionary of icao24 to aircraft IDs self._aircraft_id_map = {} # dictionary of icao24 to aircraft IDs
self._aircraft_id_count = 0 # Current Aircraft ID count self._aircraft_id_count = 0 # Current Aircraft ID count
self._cpr = cprdec
#it could be cleaner if there were separate output_* fns
#but this works
for i in (0, 4, 5, 11, 17):
pub.subscribe("type%i_dl" % i, output)
#spawn thread to add new connections as they come in
self._runner = dumb_task_runner(self.add_pending_conns, 0.1)
def __del__(self): def __del__(self):
self._s.close() self._s.close()
@@ -124,15 +94,14 @@ class output_sbs1:
elif fs == 3: elif fs == 3:
return "1,0,0,1" return "1,0,0,1"
elif fs == 4: elif fs == 4:
return "1,0,1," return "1,0,0,"
elif fs == 5: elif fs == 5:
return "0,0,1," return "0,0,0,"
else: else:
return ",,," return ",,,"
def parse(self, msg): def parse(self, msg):
#assembles a SBS-1-style output string from the received message #assembles a SBS-1-style output string from the received message
msgtype = msg.data["df"] msgtype = msg.data["df"]
outmsg = None outmsg = None
@@ -152,8 +121,9 @@ class output_sbs1:
def pp0(self, shortdata, ecc): def pp0(self, shortdata, ecc):
[datestr, timestr] = self.current_time() [datestr, timestr] = self.current_time()
[vs, cc, sl, ri, altitude] = self.parse0(shortdata)
aircraft_id = self.get_aircraft_id(ecc) aircraft_id = self.get_aircraft_id(ecc)
retstr = "MSG,7,0,%i,%06X,%i,%s,%s,%s,%s,,%s,,,,,,,,,," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr, air_modes.decode_alt(shortdata["ac"], True)) retstr = "MSG,7,0,%i,%06X,%i,%s,%s,%s,%s,,%s,,,,,,,,,," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr, altitude)
if vs: if vs:
retstr += "1\r\n" retstr += "1\r\n"
else: else:
@@ -162,20 +132,24 @@ class output_sbs1:
def pp4(self, shortdata, ecc): def pp4(self, shortdata, ecc):
[datestr, timestr] = self.current_time() [datestr, timestr] = self.current_time()
[fs, dr, um, altitude] = self.parse4(shortdata)
aircraft_id = self.get_aircraft_id(ecc) aircraft_id = self.get_aircraft_id(ecc)
retstr = "MSG,5,0,%i,%06X,%i,%s,%s,%s,%s,,%s,,,,,,," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr, air_modes.decode_alt(shortdata["ac"], True)) retstr = "MSG,5,0,%i,%06X,%i,%s,%s,%s,%s,,%s,,,,,,," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr, altitude)
return retstr + self.decode_fs(shortdata["fs"]) + "\r\n" return retstr + self.decode_fs(fs) + "\r\n"
def pp5(self, shortdata, ecc): def pp5(self, shortdata, ecc):
# I'm not sure what to do with the identiifcation shortdata & 0x1FFF
[datestr, timestr] = self.current_time() [datestr, timestr] = self.current_time()
[fs, dr, um, ident] = self.parse5(shortdata)
aircraft_id = self.get_aircraft_id(ecc) aircraft_id = self.get_aircraft_id(ecc)
retstr = "MSG,6,0,%i,%06X,%i,%s,%s,%s,%s,,,,,,,,%04i," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr, air_modes.decode_id(shortdata["id"])) retstr = "MSG,6,0,%i,%06X,%i,%s,%s,%s,%s,,,,,,,,," % (aircraft_id, ecc, aircraft_id+100, datestr, timestr, datestr, timestr)
return retstr + self.decode_fs(shortdata["fs"]) + "\r\n" return retstr + self.decode_fs(fs) + "\r\n"
def pp11(self, shortdata, ecc): def pp11(self, shortdata, ecc):
[datestr, timestr] = self.current_time() [datestr, timestr] = self.current_time()
[icao24, interrogator, ca] = self.parse11(shortdata, ecc)
aircraft_id = self.get_aircraft_id(icao24) aircraft_id = self.get_aircraft_id(icao24)
return "MSG,8,0,%i,%06X,%i,%s,%s,%s,%s,,,,,,,,,,,,\r\n" % (aircraft_id, shortdata["aa"], aircraft_id+100, datestr, timestr, datestr, timestr) return "MSG,8,0,%i,%06X,%i,%s,%s,%s,%s,,,,,,,,,,,,\r\n" % (aircraft_id, icao24, aircraft_id+100, datestr, timestr, datestr, timestr)
def pp17(self, data): def pp17(self, data):
icao24 = data["aa"] icao24 = data["aa"]
@@ -189,12 +163,12 @@ class output_sbs1:
if bdsreg == 0x08: if bdsreg == 0x08:
# Aircraft Identification # Aircraft Identification
(msg, typestring) = air_modes.parseBDS08(data) (msg, typestring) = self.parseBDS08(data)
retstr = "MSG,1,0,%i,%06X,%i,%s,%s,%s,%s,%s,,,,,,,,,,,\r\n" % (aircraft_id, icao24, aircraft_id+100, datestr, timestr, datestr, timestr, msg) retstr = "MSG,1,0,%i,%06X,%i,%s,%s,%s,%s,%s,,,,,,,,,,,\r\n" % (aircraft_id, icao24, aircraft_id+100, datestr, timestr, datestr, timestr, msg)
elif bdsreg == 0x06: elif bdsreg == 0x06:
# Surface position measurement # Surface position measurement
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS06(data, self._cpr) [ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(data)
altitude = 0 altitude = 0
if decoded_lat is None: #no unambiguously valid position available if decoded_lat is None: #no unambiguously valid position available
retstr = None retstr = None
@@ -204,7 +178,7 @@ class output_sbs1:
elif bdsreg == 0x05: elif bdsreg == 0x05:
# Airborne position measurements # Airborne position measurements
# WRONG (rnge, bearing), is this still true? # WRONG (rnge, bearing), is this still true?
[altitude, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS05(data, self._cpr) [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(data)
if decoded_lat is None: #no unambiguously valid position available if decoded_lat is None: #no unambiguously valid position available
retstr = None retstr = None
else: else:
@@ -215,7 +189,7 @@ class output_sbs1:
# WRONG (heading, vert_spd), Is this still true? # WRONG (heading, vert_spd), Is this still true?
subtype = data["bds09"].get_type() subtype = data["bds09"].get_type()
if subtype == 0 or subtype == 1: if subtype == 0 or subtype == 1:
parser = air_modes.parseBDS09_0 if subtype == 0 else air_modes.parseBDS09_1 parser = self.parseBDS09_0 if subtype == 0 else self.parseBDS09_1
[velocity, heading, vert_spd] = parser(data) [velocity, heading, vert_spd] = parser(data)
retstr = "MSG,4,0,%i,%06X,%i,%s,%s,%s,%s,,,%.1f,%.1f,,,%i,,,,,\r\n" % (aircraft_id, icao24, aircraft_id+100, datestr, timestr, datestr, timestr, velocity, heading, vert_spd) retstr = "MSG,4,0,%i,%06X,%i,%s,%s,%s,%s,,,%.1f,%.1f,,,%i,,,,,\r\n" % (aircraft_id, icao24, aircraft_id+100, datestr, timestr, datestr, timestr, velocity, heading, vert_spd)

View File

@@ -24,63 +24,64 @@ from string import split, join
import air_modes import air_modes
import sqlite3 import sqlite3
from air_modes.exceptions import * from air_modes.exceptions import *
from gnuradio.gr.pubsub import pubsub
class output_sql: class output_sql(air_modes.parse):
def __init__(self, cpr, filename, lock, publisher): def __init__(self, mypos, filename, lock):
#pubsub.__init__(self) air_modes.parse.__init__(self, mypos)
self._cpr = cpr
self._lock = lock;
#create the database
self.filename = filename
self._db = sqlite3.connect(filename)
#now execute a schema to create the tables you need
c = self._db.cursor()
query = """CREATE TABLE IF NOT EXISTS "positions" (
"icao" INTEGER KEY NOT NULL,
"seen" DATETIME NOT NULL,
"alt" INTEGER,
"lat" REAL,
"lon" REAL
);"""
c.execute(query)
query = """CREATE TABLE IF NOT EXISTS "vectors" (
"icao" INTEGER KEY NOT NULL,
"seen" DATETIME NOT NULL,
"speed" REAL,
"heading" REAL,
"vertical" REAL
);"""
c.execute(query)
query = """CREATE TABLE IF NOT EXISTS "ident" (
"icao" INTEGER PRIMARY KEY NOT NULL,
"ident" TEXT NOT NULL,
"type" TEXT NOT NULL
);"""
c.execute(query)
c.close()
self._db.commit()
#we close the db conn now to reopen it in the output() thread context.
self._db.close()
self._db = None
publisher.subscribe("type17_dl", self.insert)
def insert(self, message): self._lock = lock
with self._lock:
#create the database
self.filename = filename
self.db = sqlite3.connect(filename)
#now execute a schema to create the tables you need
c = self.db.cursor()
query = """CREATE TABLE IF NOT EXISTS "positions" (
"icao" INTEGER KEY NOT NULL,
"seen" TEXT NOT NULL,
"alt" INTEGER,
"lat" REAL,
"lon" REAL
);"""
c.execute(query)
query = """CREATE TABLE IF NOT EXISTS "vectors" (
"icao" INTEGER KEY NOT NULL,
"seen" TEXT NOT NULL,
"speed" REAL,
"heading" REAL,
"vertical" REAL
);"""
c.execute(query)
query = """CREATE TABLE IF NOT EXISTS "ident" (
"icao" INTEGER PRIMARY KEY NOT NULL,
"ident" TEXT NOT NULL
);"""
c.execute(query)
c.close()
self.db.commit()
#we close the db conn now to reopen it in the output() thread context.
self.db.close()
self.db = None
def __del__(self):
self.db = None
def output(self, message):
with self._lock: with self._lock:
try: try:
#we're checking to see if the db is empty, and creating the db object #we're checking to see if the db is empty, and creating the db object
#if it is. the reason for this is so that the db writing is done within #if it is. the reason for this is so that the db writing is done within
#the thread context of output(), rather than the thread context of the #the thread context of output(), rather than the thread context of the
#constructor. #constructor. that way you can spawn a thread to do output().
if self._db is None: if self.db is None:
self._db = sqlite3.connect(self.filename) self.db = sqlite3.connect(self.filename)
query = self.make_insert_query(message) query = self.make_insert_query(message)
if query is not None: if query is not None:
c = self._db.cursor() c = self.db.cursor()
c.execute(query) c.execute(query)
c.close() c.close()
self._db.commit() self.db.commit()
except ADSBError: except ADSBError:
pass pass
@@ -92,38 +93,43 @@ class output_sql:
msgtype = msg.data["df"] msgtype = msg.data["df"]
if msgtype == 17: if msgtype == 17:
query = self.sql17(msg.data) query = self.sql17(msg.data)
#self["new_adsb"] = data["aa"] #publish change notification
return query return query
def sql17(self, data): def sql17(self, data):
icao24 = data["aa"] icao24 = data["aa"]
bdsreg = data["me"].get_type() bdsreg = data["me"].get_type()
#self["bds%.2i" % bdsreg] = icao24 #publish under "bds08", "bds06", etc.
retstr = None
if bdsreg == 0x08: if bdsreg == 0x08:
(msg, typename) = self.parseBDS08(data) (msg, typename) = self.parseBDS08(data)
return "INSERT OR REPLACE INTO ident (icao, ident, type) VALUES (%i, '%s', '%s')" % (icao24, msg, typename) retstr = "INSERT OR REPLACE INTO ident (icao, ident) VALUES (" + "%i" % icao24 + ", '" + msg + "')"
elif bdsreg == 0x06: elif bdsreg == 0x06:
[ground_track, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS06(data, self._cpr) [ground_track, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS06(data)
altitude = 0 altitude = 0
if decoded_lat is None: #no unambiguously valid position available if decoded_lat is None: #no unambiguously valid position available
raise CPRNoPositionError retstr = None
else: else:
return "INSERT INTO positions (icao, seen, alt, lat, lon) VALUES (%i, datetime('now'), %i, %.6f, %.6f)" % (icao24, int(altitude), decoded_lat, decoded_lon) retstr = "INSERT INTO positions (icao, seen, alt, lat, lon) VALUES (" + "%i" % icao24 + ", datetime('now'), " + str(altitude) + ", " + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ")"
elif bdsreg == 0x05: elif bdsreg == 0x05:
[altitude, decoded_lat, decoded_lon, rnge, bearing] = air_modes.parseBDS05(data, self._cpr) [altitude, decoded_lat, decoded_lon, rnge, bearing] = self.parseBDS05(data)
if decoded_lat is None: #no unambiguously valid position available if decoded_lat is None: #no unambiguously valid position available
raise CPRNoPositionError retstr = None
else: else:
return "INSERT INTO positions (icao, seen, alt, lat, lon) VALUES (%i, datetime('now'), %i, %.6f, %.6f)" % (icao24, int(altitude), decoded_lat, decoded_lon) retstr = "INSERT INTO positions (icao, seen, alt, lat, lon) VALUES (" + "%i" % icao24 + ", datetime('now'), " + str(altitude) + ", " + "%.6f" % decoded_lat + ", " + "%.6f" % decoded_lon + ")"
elif bdsreg == 0x09: elif bdsreg == 0x09:
subtype = data["bds09"].get_type() subtype = data["bds09"].get_type()
if subtype == 0: if subtype == 0:
[velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(data) [velocity, heading, vert_spd, turnrate] = self.parseBDS09_0(data)
return "INSERT INTO vectors (icao, seen, speed, heading, vertical) VALUES (%i, datetime('now'), %.0f, %.0f, %.0f)" % (icao24, velocity, heading, vert_spd) retstr = "INSERT INTO vectors (icao, seen, speed, heading, vertical) VALUES (" + "%i" % icao24 + ", datetime('now'), " + "%.0f" % velocity + ", " + "%.0f" % heading + ", " + "%.0f" % vert_spd + ")"
elif subtype == 1: elif subtype == 1:
[velocity, heading, vert_spd] = self.parseBDS09_1(data) [velocity, heading, vert_spd] = self.parseBDS09_1(data)
return "INSERT INTO vectors (icao, seen, speed, heading, vertical) VALUES (%i, datetime('now'), %.0f, %.0f, %.0f)" % (icao24, velocity, heading, vert_spd) retstr = "INSERT INTO vectors (icao, seen, speed, heading, vertical) VALUES (" + "%i" % icao24 + ", datetime('now'), " + "%.0f" % velocity + ", " + "%.0f" % heading + ", " + "%.0f" % vert_spd + ")"
else: else:
raise NoHandlerError retstr = None
return retstr

View File

@@ -21,35 +21,23 @@
from collections import namedtuple from collections import namedtuple
#this is a timestamp that preserves precision when used with UTC timestamps. #this is a timestamp that preserves precision when used with UTC timestamps
#ordinary double-precision timestamps lose significant fractional precision #ordinary double-precision timestamp would lose significant fractional precision
#when the exponent is as large as necessary for UTC. #when the mantissa is as large as necessary for UTC timestamps.
class stamp: class stamp:
def __init__(self, secs, frac_secs): def __init__(self, secs, frac_secs):
self.secs = secs self.secs = secs
self.frac_secs = frac_secs self.frac_secs = frac_secs
self.secs += int(self.frac_secs)
self.frac_secs -= int(self.frac_secs)
def __lt__(self, other): def __lt__(self, other):
if isinstance(other, self.__class__): if self.secs == other.secs:
if self.secs == other.secs: return self.frac_secs < other.frac_secs
return self.frac_secs < other.frac_secs
else:
return self.secs < other.secs
elif isinstance(other, float):
return float(self) > other
else: else:
raise TypeError return self.secs < other.secs
def __gt__(self, other): def __gt__(self, other):
if type(other) is type(self): if self.secs == other.secs:
if self.secs == other.secs: return self.frac_secs > other.frac_secs
return self.frac_secs > other.frac_secs
else:
return self.secs > other.secs
elif type(other) is type(float):
return float(self) > other
else: else:
raise TypeError return self.secs > other.secs
def __eq__(self, other): def __eq__(self, other):
if isinstance(other, self.__class__): if isinstance(other, self.__class__):
return self.secs == other.secs and self.frac_secs == other.frac_secs return self.secs == other.secs and self.frac_secs == other.frac_secs
@@ -58,50 +46,17 @@ class stamp:
else: else:
raise TypeError raise TypeError
def __ne__(self, other): def __ne__(self, other):
return not (self == other) return self.secs != other.secs or self.frac_secs != other.frac_secs
def __le__(self, other): def __le__(self, other):
return (self == other) or (self < other) return (self == other) or (self < other)
def __ge__(self, other): def __ge__(self, other):
return (self == other) or (self > other) return (self == other) or (self > other)
def __add__(self, other):
if isinstance(other, self.__class__):
ipart = self.secs + other.secs
fpart = self.frac_secs + other.frac_secs
return stamp(ipart, fpart)
elif isinstance(other, float):
return self + stamp(0, other)
elif isinstance(other, int):
return self + stamp(other, 0)
else:
raise TypeError
def __sub__(self, other):
if isinstance(other, self.__class__):
ipart = self.secs - other.secs
fpart = self.frac_secs - other.frac_secs
return stamp(ipart, fpart)
elif isinstance(other, float):
return self - stamp(0, other)
elif isinstance(other, int):
return self - stamp(other, 0)
else:
raise TypeError
#to ensure we don't hash by stamp #to ensure we don't hash by stamp
#TODO fixme with a reasonable hash in case you feel like you'd hash by stamp
__hash__ = None __hash__ = None
#good to within ms for comparison #good to within ms for comparison
def __float__(self): def __float__(self):
return self.secs + self.frac_secs return self.secs + self.frac_secs
def __str__(self): modes_report = namedtuple('modes_report', ['data', 'ecc', 'reference', 'timestamp'])
return "%f" % float(self)
#a Mode S report including the modes_reply data object
modes_report = namedtuple('modes_report', ['data', 'ecc', 'rssi', 'timestamp'])
#lat, lon, alt
#TODO: a position class internally represented as ECEF XYZ which can easily be used for multilateration and distance calculation
llh = namedtuple('llh', ['lat', 'lon', 'alt'])
mlat_report = namedtuple('mlat_report', ['data', 'nreps', 'timestamp', 'llh', 'hdop', 'vdop'])

View File

@@ -1,140 +0,0 @@
# Copyright 2013 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.
#this serves as a bridge between ZMQ subscriber and the GR pubsub callbacks interface
#creates a thread, publishes socket data to pubsub subscribers
#just a convenient way to create an aggregating socket with callbacks on receive
#can use this for inproc:// signalling with minimal overhead
#not sure if it's a good idea to use this yet
import time
import threading
import zmq
from gnuradio.gr.pubsub import pubsub
import Queue
class zmq_pubsub_iface(threading.Thread):
def __init__(self, context, subaddr=None, pubaddr=None):
threading.Thread.__init__(self)
#private data
self._queue = Queue.Queue()
self._subsocket = context.socket(zmq.SUB)
self._pubsocket = context.socket(zmq.PUB)
self._subaddr = subaddr
self._pubaddr = pubaddr
if type(self._subaddr) is str:
self._subaddr = [self._subaddr]
if type(self._pubaddr) is str:
self._pubaddr = [self._pubaddr]
self._sub_connected = False
self._pubsub = pubsub()
if self._pubaddr is not None:
for addr in self._pubaddr:
self._pubsocket.bind(addr)
self._poller = zmq.Poller()
self._poller.register(self._subsocket, zmq.POLLIN)
#public data
self.shutdown = threading.Event()
self.finished = threading.Event()
#init
self.setDaemon(True)
self.start()
def subscribe(self, key, subscriber):
if not self._sub_connected:
if not self._subaddr:
raise Exception("No subscriber address set")
for addr in self._subaddr:
self._subsocket.connect(addr)
self._sub_connected = True
self._subsocket.setsockopt(zmq.SUBSCRIBE, key)
self._pubsub.subscribe(key, subscriber)
def unsubscribe(self, key, subscriber):
self._subsocket.setsockopt(zmq.UNSUBSCRIBE, key)
self._pubsub.unsubscribe(key, subscriber)
#executed from the thread context(s) of the caller(s)
#so we use a queue to push sending into the run loop
#since sockets must be used in the thread they were created in
def __setitem__(self, key, val):
if not self._pubaddr:
raise Exception("No publisher address set")
if not self.shutdown.is_set():
self._queue.put([key, val])
def __getitem__(self, key):
return self._pubsub[key]
def run(self):
done = False
while not self.shutdown.is_set() and not done:
if self.shutdown.is_set():
done = True
#send
while not self._queue.empty():
self._pubsocket.send_multipart(self._queue.get())
#receive
if self._sub_connected:
socks = dict(self._poller.poll(timeout=0))
while self._subsocket in socks \
and socks[self._subsocket] == zmq.POLLIN:
[address, msg] = self._subsocket.recv_multipart()
self._pubsub[address] = msg
socks = dict(self._poller.poll(timeout=0))
#snooze
if not done:
time.sleep(0.1)
self._subsocket.close()
self._pubsocket.close()
self.finished.set()
def close(self):
self.shutdown.set()
#self._queue.join() #why does this block forever
self.finished.wait(0.2)
def pr(x):
print x
if __name__ == "__main__":
#create socket pair
context = zmq.Context(1)
sock1 = zmq_pubsub_iface(context, subaddr="inproc://sock2-pub", pubaddr="inproc://sock1-pub")
sock2 = zmq_pubsub_iface(context, subaddr="inproc://sock1-pub", pubaddr=["inproc://sock2-pub", "tcp://*:5433"])
sock3 = zmq_pubsub_iface(context, subaddr="tcp://localhost:5433", pubaddr=None)
sock1.subscribe("data1", pr)
sock2.subscribe("data2", pr)
sock3.subscribe("data3", pr)
for i in range(10):
sock1["data2"] = "HOWDY"
sock2["data3"] = "DRAW"
sock2["data1"] = "PARDNER"
time.sleep(0.1)
time.sleep(0.1)
sock1.close()
sock2.close()
sock3.close()

View File

@@ -1,4 +1,4 @@
# Copyright 2011,2013 Free Software Foundation, Inc. # Copyright 2011 Free Software Foundation, Inc.
# #
# This file is part of GNU Radio # This file is part of GNU Radio
# #
@@ -31,17 +31,14 @@ if(NOT PYQT4_FOUND)
return() return()
endif() endif()
if(NOT PYUIC4_FOUND)
message(STATUS "pyuic4 not found, not installing GUI application")
return()
endif()
find_package(Qwt) find_package(Qwt)
if(NOT QWT_FOUND) if(NOT QWT_FOUND)
message(STATUS "Qwt not found, not installing GUI application") message(STATUS "Qwt not found, not installing GUI application")
return() return()
endif() endif()
set(PYUIC4_COMPILE pyuic4)
set(RX_UI_SRC ${CMAKE_CURRENT_SOURCE_DIR}/modes_rx.ui) set(RX_UI_SRC ${CMAKE_CURRENT_SOURCE_DIR}/modes_rx.ui)
set(RX_UI_PY_PRE_MASSAGE ${CMAKE_CURRENT_BINARY_DIR}/modes_rx_ui_borked.py) set(RX_UI_PY_PRE_MASSAGE ${CMAKE_CURRENT_BINARY_DIR}/modes_rx_ui_borked.py)
set(RX_UI_PY ${CMAKE_CURRENT_BINARY_DIR}/modes_rx_ui.py) set(RX_UI_PY ${CMAKE_CURRENT_BINARY_DIR}/modes_rx_ui.py)
@@ -52,7 +49,7 @@ add_custom_target(rx_ui ALL
) )
add_custom_command(OUTPUT ${RX_UI_PY_PRE_MASSAGE} add_custom_command(OUTPUT ${RX_UI_PY_PRE_MASSAGE}
COMMAND ${PYUIC4_EXECUTABLE} ${RX_UI_SRC} > ${RX_UI_PY_PRE_MASSAGE} COMMAND ${PYUIC4_COMPILE} ${RX_UI_SRC} > ${RX_UI_PY_PRE_MASSAGE}
MAIN_DEPENDENCY ${RX_UI_SRC} MAIN_DEPENDENCY ${RX_UI_SRC}
) )

View File

@@ -365,7 +365,7 @@
</rect> </rect>
</property> </property>
<property name="text"> <property name="text">
<string>TCP</string> <string>Raw</string>
</property> </property>
</widget> </widget>
<widget class="QLineEdit" name="line_kmlfilename"> <widget class="QLineEdit" name="line_kmlfilename">
@@ -856,7 +856,7 @@
<string>Climb</string> <string>Climb</string>
</property> </property>
</widget> </widget>
<widget class="QwtCompass" name="compass_heading" native="true"> <widget class="QwtCompass" name="compass_heading">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>200</x> <x>200</x>
@@ -865,10 +865,10 @@
<height>91</height> <height>91</height>
</rect> </rect>
</property> </property>
<property name="readOnly" stdset="0"> <property name="readOnly">
<bool>true</bool> <bool>true</bool>
</property> </property>
<property name="lineWidth" stdset="0"> <property name="lineWidth">
<number>4</number> <number>4</number>
</property> </property>
</widget> </widget>
@@ -901,7 +901,7 @@
<bool>true</bool> <bool>true</bool>
</property> </property>
</widget> </widget>
<widget class="QwtCompass" name="compass_bearing" native="true"> <widget class="QwtCompass" name="compass_bearing">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>-20</x> <x>-20</x>
@@ -910,10 +910,10 @@
<height>91</height> <height>91</height>
</rect> </rect>
</property> </property>
<property name="readOnly" stdset="0"> <property name="readOnly">
<bool>true</bool> <bool>true</bool>
</property> </property>
<property name="lineWidth" stdset="0"> <property name="lineWidth">
<number>4</number> <number>4</number>
</property> </property>
</widget> </widget>
@@ -928,16 +928,6 @@
</item> </item>
</layout> </layout>
</widget> </widget>
<widget class="QWidget" name="map_tab">
<attribute name="title">
<string>Map</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_8">
<item row="0" column="0">
<widget class="QWebView" name="mapView" native="true"/>
</item>
</layout>
</widget>
<widget class="QWidget" name="livedatatab"> <widget class="QWidget" name="livedatatab">
<property name="sizePolicy"> <property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding"> <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
@@ -1026,6 +1016,11 @@
<widget class="QStatusBar" name="statusbar"/> <widget class="QStatusBar" name="statusbar"/>
</widget> </widget>
<customwidgets> <customwidgets>
<customwidget>
<class>QwtCompass</class>
<extends>QwtDial</extends>
<header>qwt_compass.h</header>
</customwidget>
<customwidget> <customwidget>
<class>QwtDial</class> <class>QwtDial</class>
<extends>QWidget</extends> <extends>QWidget</extends>
@@ -1037,17 +1032,6 @@
<header location="global">air_modes/az_map</header> <header location="global">air_modes/az_map</header>
<container>1</container> <container>1</container>
</customwidget> </customwidget>
<customwidget>
<class>QwtCompass</class>
<extends>QwtDial</extends>
<header>qwt_compass.h</header>
</customwidget>
<customwidget>
<class>QWebView</class>
<extends>QWidget</extends>
<header location="global">QtWebKit/qwebview.h</header>
<container>1</container>
</customwidget>
</customwidgets> </customwidgets>
<resources/> <resources/>
<connections/> <connections/>

View File

@@ -31,8 +31,11 @@ include(GrPython)
######################################################################## ########################################################################
# Setup swig generation # Setup swig generation
######################################################################## ########################################################################
foreach(incdir ${GNURADIO_RUNTIME_INCLUDE_DIRS}) foreach(incdir ${GNURADIO_CORE_INCLUDE_DIRS})
list(APPEND GR_SWIG_INCLUDE_DIRS ${incdir}/gnuradio/swig) list(APPEND GR_SWIG_INCLUDE_DIRS ${incdir}/swig)
endforeach(incdir)
foreach(incdir ${GRUEL_INCLUDE_DIRS})
list(APPEND GR_SWIG_INCLUDE_DIRS ${incdir}/gruel/swig)
endforeach(incdir) endforeach(incdir)
set(GR_SWIG_LIBRARIES air_modes) set(GR_SWIG_LIBRARIES air_modes)

View File

@@ -5,7 +5,7 @@
%{ %{
#include "air_modes_preamble.h" #include "air_modes_preamble.h"
#include "air_modes_slicer.h" #include "air_modes_slicer.h"
#include <gnuradio/msg_queue.h> #include <gr_msg_queue.h>
%} %}
// ---------------------------------------------------------------- // ----------------------------------------------------------------
@@ -21,24 +21,20 @@ GR_SWIG_BLOCK_MAGIC(air,modes_preamble);
air_modes_preamble_sptr air_make_modes_preamble (int channel_rate, float threshold_db); air_modes_preamble_sptr air_make_modes_preamble (int channel_rate, float threshold_db);
class air_modes_preamble : public gr::sync_block class air_modes_preamble : public gr_sync_block
{ {
set_rate(int channel_rate);
set_threshold(float threshold_db);
int get_threshold(void);
private: private:
air_modes_preamble (int channel_rate, float threshold_db); air_modes_preamble (int channel_rate, float threshold_db);
}; };
GR_SWIG_BLOCK_MAGIC(air,modes_slicer); GR_SWIG_BLOCK_MAGIC(air,modes_slicer);
air_modes_slicer_sptr air_make_modes_slicer (int channel_rate, gr::msg_queue::sptr queue); air_modes_slicer_sptr air_make_modes_slicer (int channel_rate, gr_msg_queue_sptr queue);
class air_modes_slicer : public gr::block class air_modes_slicer : public gr_block
{ {
set_rate(int channel_rate);
private: private:
air_modes_slicer (int channel_rate, gr::msg_queue::sptr queue); air_modes_slicer (int channel_rate, gr_msg_queue_sptr queue);
}; };
// ---------------------------------------------------------------- // ----------------------------------------------------------------