def __init__(self, uuid, user, connection, clock, position_llh, privacy,
connection_info):
self.uuid = uuid
self.user = user
self.connection = connection
self.clock = clock
self.last_clock_reset = time.monotonic()
self.clock_reset_counter = 0
self.position_llh = position_llh
self.position = geodesy.llh2ecef(position_llh)
self.privacy = privacy
self.connection_info = connection_info
self.dead = False
self.sync_count = 0
self.sync_peers = 0 # number of peers hopefully updated live
self.peer_count = 0 # only updated when dumping state
self.last_rate_report = None
self.tracking = set()
self.adsb_seen = set()
self.sync_interest = set()
self.mlat_interest = set()
self.requested = set()
self.offX = 0.016 * random.randrange(-1, 2, 2)
self.offY = 0.016 * random.randrange(-1, 2, 2)
self.distance = {}
# Receivers with bad_syncs>0 are not used to calculate positions
self.bad_syncs = 0
self.sync_range_exceeded = 0
def receiver_location_update(self, receiver, position_llh):
"""Note that a given receiver has moved."""
print("Receiver {r} Position Update {p}".format(r=receiver.user,
p=position_llh))
receiver.position_llh = position_llh
receiver.position = geodesy.llh2ecef(position_llh)
self._compute_interstation_distances(receiver)
def __init__(self, uid, user, connection, clock_type, position_llh,
privacy, connection_info, uuid, coordinator, clock_tracker):
self.uid = uid
self.uuid = uuid
self.user = user
self.connection = connection
self.coordinator = coordinator
self.clock_tracker = clock_tracker
self.clock = clocktrack.make_clock(clock_type)
self.epoch = None
if clock_type == 'radarcape_gps':
self.epoch = 'gps_midnight'
self.last_clock_reset = time.time()
self.clock_reset_counter = 0
self.position_llh = position_llh
self.position = geodesy.llh2ecef(position_llh)
self.privacy = privacy
self.connection_info = connection_info
self.dead = False
self.connectedSince = time.time()
self.num_outliers = 0
self.num_syncs = 0
self.outlier_percent_rolling = 0
self.sync_peers = array.array(
'i', [0, 0, 0, 0, 0]) # number of peers per distance category
self.peer_count = 0 # only updated when dumping state
self.last_rate_report = None
self.tracking = set()
self.adsb_seen = set()
self.sync_interest = set()
self.mlat_interest = set()
self.requested = set()
self.offX = 1 / 20 * random.random()
self.offY = 1 / 20 * random.random()
self.distance = {}
# timestamp this receiver last synced with the result being a valid clock pair
self.last_sync = 0
# Receivers with bad_syncs>0 are not used to calculate positions
self.bad_syncs = 0
self.sync_range_exceeded = 0
self.recent_pair_jumps = 0
self.focus = False
def __init__(self, uuid, user, connection, clock, position_llh, privacy, connection_info):
self.uuid = uuid
self.user = user
self.connection = connection
self.clock = clock
self.position_llh = position_llh
self.position = geodesy.llh2ecef(position_llh)
self.privacy = privacy
self.connection_info = connection_info
self.dead = False
self.sync_count = 0
self.last_rate_report = None
self.tracking = set()
self.sync_interest = set()
self.mlat_interest = set()
self.requested = set()
self.distance = {}
def __init__(self, uuid, user, connection, clock, position_llh, privacy,
connection_info):
self.uuid = uuid
self.user = user
self.connection = connection
self.clock = clock
self.position_llh = position_llh
self.position = geodesy.llh2ecef(position_llh)
self.privacy = privacy
self.connection_info = connection_info
self.dead = False
self.sync_count = 0
self.last_rate_report = None
self.tracking = set()
self.sync_interest = set()
self.mlat_interest = set()
self.requested = set()
self.distance = {}
def __init__(self, uuid, user, connection, clock, position_llh, privacy, connection_info):
self.uuid = uuid
self.user = user
self.connection = connection
self.clock = clock
self.position_llh = position_llh
self.position = geodesy.llh2ecef(position_llh)
self.privacy = privacy
self.connection_info = connection_info
self.dead = False
self.sync_count = 0
self.peer_count = 0 # only updated when dumping state
self.last_rate_report = None
self.tracking = set()
self.sync_interest = set()
self.mlat_interest = set()
self.requested = set()
self.distance = {}
# Receivers with bad_syncs>0 are not used to calculate positions
self.bad_syncs = 0
def receiver_sync(self, receiver,
even_time, odd_time,
even_message, odd_message):
"""
Called by the coordinator to handle a sync message from a receiver.
Looks for a suitable existing sync point and, if there is one, does
synchronization between this receiver and the existing receivers
associated with the sync point.
Otherwise, validates the message pair and, if it is suitable, creates a
new sync point for it.
receiver: the receiver reporting the sync message
even_message: a DF17 airborne position message with F=0
odd_message: a DF17 airborne position message with F=1
even_time: the time of arrival of even_message, as seen by receiver.clock
odd_time: the time of arrival of odd_message, as seen by receiver.clock
"""
# Do sanity checks.
# Messages must be within 5 seconds of each other.
if abs(even_time - odd_time) / receiver.clock.freq > 5.0:
return
# compute key and interval
if even_time < odd_time:
tA = even_time
tB = odd_time
key = (even_message, odd_message)
else:
tA = odd_time
tB = even_time
key = (odd_message, even_message)
interval = (tB - tA) / receiver.clock.freq
# do we have a suitable existing match?
syncpointlist = self.sync_points.get(key)
if syncpointlist:
for candidate in syncpointlist:
if abs(candidate.interval - interval) < 1e-3:
# interval matches within 1ms, close enough.
self._add_to_existing_syncpoint(candidate, receiver, tA, tB)
return
# No existing match. Validate the messages and maybe create a new sync point
# basic validity
even_message = modes.message.decode(even_message)
if ((not even_message or
even_message.DF != 17 or
not even_message.crc_ok or
even_message.estype != modes.message.ESType.airborne_position or
even_message.F)):
return
odd_message = modes.message.decode(odd_message)
if ((not odd_message or
odd_message.DF != 17 or
not odd_message.crc_ok or
odd_message.estype != modes.message.ESType.airborne_position or
not odd_message.F)):
return
if even_message.address != odd_message.address:
return
# quality checks
if even_message.nuc < 6 or even_message.altitude is None:
return
if odd_message.nuc < 6 or odd_message.altitude is None:
return
if abs(even_message.altitude - odd_message.altitude) > 5000:
return
# find global positions
try:
even_lat, even_lon, odd_lat, odd_lon = modes.cpr.decode(even_message.LAT,
even_message.LON,
odd_message.LAT,
odd_message.LON)
except ValueError:
# CPR failed
return
# convert to ECEF, do range checks
even_ecef = geodesy.llh2ecef((even_lat,
even_lon,
even_message.altitude * constants.FTOM))
if geodesy.ecef_distance(even_ecef, receiver.position) > config.MAX_RANGE:
logging.info("{a:06X}: receiver range check (even) failed".format(a=even_message.address))
return
odd_ecef = geodesy.llh2ecef((odd_lat,
odd_lon,
odd_message.altitude * constants.FTOM))
if geodesy.ecef_distance(odd_ecef, receiver.position) > config.MAX_RANGE:
logging.info("{a:06X}: receiver range check (odd) failed".format(a=odd_message.address))
return
if geodesy.ecef_distance(even_ecef, odd_ecef) > config.MAX_INTERMESSAGE_RANGE:
logging.info("{a:06X}: intermessage range check failed".format(a=even_message.address))
return
# valid. Create a new sync point.
if even_time < odd_time:
syncpoint = SyncPoint(even_message.address, even_ecef, odd_ecef, interval)
else:
syncpoint = SyncPoint(even_message.address, odd_ecef, even_ecef, interval)
syncpoint.receivers.append([receiver, tA, tB, False])
if not syncpointlist:
syncpointlist = self.sync_points[key] = []
syncpointlist.append(syncpoint)
# schedule cleanup of the syncpoint after 2 seconds -
# we should have seen all copies of those messages by
# then.
asyncio.get_event_loop().call_later(
2.0,
functools.partial(self._cleanup_syncpoint,
key=key,
syncpoint=syncpoint))
def receiver_location_update(self, receiver, position_llh):
"""Note that a given receiver has moved."""
receiver.position_llh = position_llh
receiver.position = geodesy.llh2ecef(position_llh)
self._compute_interstation_distances(receiver)
def _resolve(self, group):
del self.pending[group.message]
# less than 3 messages -> no go
if len(group.copies) < 3:
return
decoded = modes_cython.message.decode(group.message)
if not decoded or not decoded.address:
return
ac = self.tracker.aircraft.get(decoded.address)
if not ac:
return
now = time.time()
ac.seen = now
ac.mlat_message_count += 1
self.coordinator.stats_valid_groups += 1
if not ac.allow_mlat:
glogger.info("not doing mlat for {0:06x}, wrong partition!".format(ac.icao))
return
# When we've seen a few copies of the same message, it's
# probably correct. Update the tracker with newly seen
# altitudes, squawks, callsigns.
if decoded.altitude is not None and decoded.altitude > -1500 and decoded.altitude < 75000:
if (not ac.last_altitude_time
or (group.first_seen > ac.last_altitude_time
and (group.first_seen - ac.last_altitude_time > 15 or abs(ac.altitude - decoded.altitude) < 4000)
)
):
ac.altitude = decoded.altitude
ac.last_altitude_time = group.first_seen
new_hist = []
for ts, old_alt in ac.alt_history:
if group.first_seen - ts < 20.0:
new_hist.append((ts, old_alt))
ac.alt_history = new_hist
ac.alt_history.append((group.first_seen, decoded.altitude))
ts_diff = group.first_seen - new_hist[0][0]
if ts_diff > 10:
# fpm
new_vrate = (decoded.altitude - new_hist[0][1]) / (ts_diff / 60.0)
if ac.vrate and group.first_seen - ac.vrate_time < 15:
ac.vrate = int(ac.vrate + 0.3 * (new_vrate - ac.vrate))
else:
ac.vrate = int(new_vrate)
ac.vrate_time = group.first_seen
if decoded.squawk is not None:
ac.squawk = decoded.squawk
if decoded.callsign is not None:
ac.callsign = decoded.callsign
if now - ac.last_resolve_attempt < config.RESOLVE_INTERVAL:
return
ac.last_resolve_attempt = now
# find old result, if present
if ac.last_result_position is None or (group.first_seen - ac.last_result_time) > 120:
last_result_position = None
last_result_var = 1e9
last_result_dof = 0
last_result_time = group.first_seen - 120
else:
last_result_position = ac.last_result_position
last_result_var = ac.last_result_var
last_result_dof = ac.last_result_dof
last_result_time = ac.last_result_time
elapsed = group.first_seen - last_result_time
if elapsed < 0:
elapsed = 0
if elapsed < config.RESOLVE_BACKOFF:
return
# find altitude
if (
ac.altitude is None
or ac.altitude < config.MIN_ALT
or ac.altitude > config.MAX_ALT
or group.first_seen > ac.last_altitude_time + 45
):
altitude = None
altitude_dof = 0
else:
altitude = ac.altitude * constants.FTOM
altitude_dof = 1
len_copies = len(group.copies)
max_dof = len_copies + altitude_dof - 4
if max_dof < 0:
return
if elapsed < 2 * config.RESOLVE_BACKOFF and max_dof < last_result_dof - elapsed + 0.5:
return
# construct a map of receiver -> list of timestamps
timestamp_map = {}
for receiver, timestamp, utc in group.copies:
timestamp_map.setdefault(receiver, []).append((timestamp, utc))
# check for minimum needed receivers
dof = len(timestamp_map) + altitude_dof - 4
if dof < 0:
return
if elapsed < 2 * config.RESOLVE_BACKOFF and dof < last_result_dof - elapsed + 0.5:
return
self.coordinator.stats_normalize += 1
# normalize timestamps. This returns a list of timestamp maps;
# within each map, the timestamp values are comparable to each other.
try:
components = clocktrack.normalize2(clocktracker=self.clock_tracker,
timestamp_map=timestamp_map)
except Exception as e:
traceback.print_exc()
return
# cluster timestamps into clusters that are probably copies of the
# same transmission.
clusters = []
min_component_size = 4 - altitude_dof
for component in components:
if len(component) >= min_component_size: # don't bother with orphan components at all
clusters.extend(_cluster_timestamps(component, min_component_size))
if not clusters:
return
# start from the most recent, largest, cluster
result = None
error = None
clusters.sort(key=lambda x: (x[0], x[1]))
while clusters and not result:
distinct, cluster_utc, cluster = clusters.pop()
# accept fewer receivers after 10s
# accept more receivers immediately
elapsed = cluster_utc - last_result_time
dof = distinct + altitude_dof - 4
if elapsed < 2 and dof < last_result_dof - elapsed + 0.5:
return
# assume 250ft accuracy at the time it is reported
# (this bundles up both the measurement error, and
# that we don't adjust for local pressure)
#
# Then degrade the accuracy over time at ~4000fpm
if decoded.altitude is not None:
altitude_error = 250 * constants.FTOM
elif altitude is not None:
altitude_error = (250 + (cluster_utc - ac.last_altitude_time) * 70) * constants.FTOM
else:
altitude_error = None
cluster.sort(key=operator.itemgetter(1)) # sort by increasing timestamp (todo: just assume descending..)
if elapsed > 30 and dof == 0:
continue
if elapsed < 60:
initial_guess = last_result_position
else:
initial_guess = cluster[0][0].position
self.coordinator.stats_solve_attempt += 1
r = solver.solve(cluster, altitude, altitude_error, initial_guess)
if r:
# estimate the error
ecef, ecef_cov = r
max_error = 10e3 # 10 km
if ecef_cov is not None:
var_est = numpy.trace(ecef_cov)
else:
# this result is suspect
var_est = max_error * max_error
#glogger.warn('{a:06X} {e:7.3f} '.format(a=decoded.address, e=999.999) + str([line[0].user for line in cluster]))
# don't use this
continue
error = int(math.sqrt(abs(var_est)))
if False and elapsed > 30 and error < 1e9:
lat, lon, alt = geodesy.ecef2llh(ecef)
ecef, ecef_cov = r
glogger.warn('{a:06X} {e:8.1f} {lat:7.3f},{lon:7.3f},{alt:5.0f} '.format(a=decoded.address, e=error/1000, lat=lat, lon=lon, alt=alt) + str([line[0].user for line in cluster]))
if error > max_error:
continue
self.coordinator.stats_solve_success += 1
# the higher the accuracy, the higher the freqency of positions that is output
if elapsed / 20 < error / max_error:
continue
self.coordinator.stats_solve_used += 1
#if elapsed < 10.0 and var_est > last_result_var * 2.25:
# # much less accurate than a recent-ish position
# continue
# accept it
result = r
if not result:
return
ecef, ecef_cov = result
ac.last_result_position = ecef
ac.last_result_var = var_est
ac.last_result_dof = dof
ac.last_result_time = cluster_utc
ac.mlat_result_count += 1
if altitude is not None:
lat, lon, _ = geodesy.ecef2llh(ecef)
# replace ecef altitude with reported altitude
ecef = geodesy.llh2ecef([lat, lon, altitude])
if ac.kalman.update(cluster_utc, cluster, altitude, altitude_error, ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
else:
_, _, solved_alt = geodesy.ecef2llh(ecef)
#glogger.info("{addr:06x} solved altitude={solved_alt:.0f}ft with dof={dof}".format(
# addr=decoded.address,
# solved_alt=solved_alt*constants.MTOF,
# dof=dof))
if ac.kalman.update(cluster_utc, cluster, solved_alt, 4000 / math.sqrt(dof + 1), ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
for handler in self.coordinator.output_handlers:
handler(cluster_utc, decoded.address,
ecef, ecef_cov,
[receiver for receiver, timestamp, error in cluster], distinct, dof,
ac.kalman, error)
# forward result to all receivers that received the raw message the result is based on
self.coordinator.forward_results(cluster_utc, decoded.address,
ecef, ecef_cov,
list(group.receivers), distinct, dof,
ac.kalman, error)
if self.pseudorange_file:
cluster_state = []
t0 = cluster[0][1]
for receiver, timestamp, variance in cluster:
cluster_state.append([round(receiver.position[0], 0),
round(receiver.position[1], 0),
round(receiver.position[2], 0),
round((timestamp-t0)*1e6, 1),
round(variance*1e12, 2)])
state = {'icao': '{a:06x}'.format(a=decoded.address),
'time': round(cluster_utc, 3),
'ecef': [round(ecef[0], 0),
round(ecef[1], 0),
round(ecef[2], 0)],
'distinct': distinct,
'dof': dof,
'cluster': cluster_state}
if ecef_cov is not None:
state['ecef_cov'] = [round(ecef_cov[0, 0], 0),
round(ecef_cov[0, 1], 0),
round(ecef_cov[0, 2], 0),
round(ecef_cov[1, 0], 0),
round(ecef_cov[1, 1], 0),
round(ecef_cov[1, 2], 0),
round(ecef_cov[2, 0], 0),
round(ecef_cov[2, 1], 0),
round(ecef_cov[2, 2], 0)]
if altitude is not None:
state['altitude'] = round(altitude, 0)
state['altitude_error'] = round(altitude_error, 0)
ujson.dump(state, self.pseudorange_file)
self.pseudorange_file.write('\n')
def solve(measurements, altitude, altitude_error, initial_guess):
"""Given a set of receive timestamps, multilaterate the position of the transmitter.
measurements: a list of (receiver, timestamp, error) tuples. Should be sorted by timestamp.
receiver.position should be the ECEF position of the receiver
timestamp should be a reception time in seconds (with an arbitrary epoch)
variance should be the estimated variance of timestamp
altitude: the reported altitude of the transmitter in _meters_, or None
altitude_error: the estimated error in altitude in meters, or None
initial_guess: an ECEF position to start the solver from
Returns None on failure, or (ecef, ecef_cov) on success, with:
ecef: the multilaterated ECEF position of the transmitter
ecef_cov: an estimate of the covariance matrix of ecef
"""
if len(measurements) + (0 if altitude is None else 1) < 4:
raise ValueError('Not enough measurements available')
glat, glng, galt = geodesy.ecef2llh(initial_guess)
if galt < config.MIN_ALT:
galt = -galt
initial_guess = geodesy.llh2ecef([glat, glng, galt])
if galt > config.MAX_ALT:
galt = config.MAX_ALT
initial_guess = geodesy.llh2ecef([glat, glng, galt])
base_timestamp = measurements[0][1]
pseudorange_data = [(receiver.position,
(timestamp - base_timestamp) * constants.Cair,
math.sqrt(variance) * constants.Cair)
for receiver, timestamp, variance in measurements]
x_guess = [initial_guess[0], initial_guess[1], initial_guess[2], 0.0]
x_est, cov_x, infodict, mesg, ler = scipy.optimize.leastsq(
_residuals,
x_guess,
args=(pseudorange_data, altitude, altitude_error),
full_output=True,
maxfev=config.SOLVER_MAXFEV)
if ler in (1, 2, 3, 4):
#glogger.info("solver success: {0} {1}".format(ler, mesg))
# Solver found a result. Validate that it makes
# some sort of physical sense.
(*position_est, offset_est) = x_est
if offset_est < 0 or offset_est > config.MAX_RANGE:
#glogger.info("solver: bad offset: {0}".formaT(offset_est))
# implausible range offset to closest receiver
return None
for receiver, timestamp, variance in measurements:
d = geodesy.ecef_distance(receiver.position, position_est)
if d > config.MAX_RANGE:
# too far from this receiver
#glogger.info("solver: bad range: {0}".format(d))
return None
if cov_x is None:
return position_est, None
else:
return position_est, cov_x[0:3, 0:3]
else:
# Solver failed
#glogger.info("solver: failed: {0} {1}".format(ler, mesg))
return None
def receiver_sync(self, receiver,
even_time, odd_time,
even_message, odd_message):
"""
Called by the coordinator to handle a sync message from a receiver.
Looks for a suitable existing sync point and, if there is one, does
synchronization between this receiver and the existing receivers
associated with the sync point.
Otherwise, validates the message pair and, if it is suitable, creates a
new sync point for it.
receiver: the receiver reporting the sync message
even_message: a DF17 airborne position message with F=0
odd_message: a DF17 airborne position message with F=1
even_time: the time of arrival of even_message, as seen by receiver.clock
odd_time: the time of arrival of odd_message, as seen by receiver.clock
"""
# Do sanity checks.
# Messages must be within 5 seconds of each other.
if abs(even_time - odd_time) / receiver.clock.freq > 5.0:
return
# compute key and interval
if even_time < odd_time:
tA = even_time
tB = odd_time
key = (even_message, odd_message)
else:
tA = odd_time
tB = even_time
key = (odd_message, even_message)
interval = (tB - tA) / receiver.clock.freq
# do we have a suitable existing match?
syncpointlist = self.sync_points.get(key)
if syncpointlist:
for candidate in syncpointlist:
if abs(candidate.interval - interval) < 1e-3:
# interval matches within 1ms, close enough.
self._add_to_existing_syncpoint(candidate, receiver, tA, tB)
return
# No existing match. Validate the messages and maybe create a new sync point
# basic validity
even_message = modes.message.decode(even_message)
if ((not even_message or
even_message.DF != 17 or
not even_message.crc_ok or
even_message.estype != modes.message.ESType.airborne_position or
even_message.F)):
return
odd_message = modes.message.decode(odd_message)
if ((not odd_message or
odd_message.DF != 17 or
not odd_message.crc_ok or
odd_message.estype != modes.message.ESType.airborne_position or
not odd_message.F)):
return
if even_message.address != odd_message.address:
return
# quality checks
if even_message.nuc < 6 or even_message.altitude is None:
return
if odd_message.nuc < 6 or odd_message.altitude is None:
return
if abs(even_message.altitude - odd_message.altitude) > 5000:
return
# find global positions
try:
even_lat, even_lon, odd_lat, odd_lon = modes.cpr.decode(even_message.LAT,
even_message.LON,
odd_message.LAT,
odd_message.LON)
except ValueError:
# CPR failed
return
# convert to ECEF, do range checks
even_ecef = geodesy.llh2ecef((even_lat,
even_lon,
even_message.altitude * constants.FTOM))
if geodesy.ecef_distance(even_ecef, receiver.position) > config.MAX_RANGE:
logging.info("{a:06X}: receiver range check (even) failed".format(a=even_message.address))
return
odd_ecef = geodesy.llh2ecef((odd_lat,
odd_lon,
odd_message.altitude * constants.FTOM))
if geodesy.ecef_distance(odd_ecef, receiver.position) > config.MAX_RANGE:
logging.info("{a:06X}: receiver range check (odd) failed".format(a=odd_message.address))
return
if geodesy.ecef_distance(even_ecef, odd_ecef) > config.MAX_INTERMESSAGE_RANGE:
logging.info("{a:06X}: intermessage range check failed".format(a=even_message.address))
return
# valid. Create a new sync point.
if even_time < odd_time:
syncpoint = SyncPoint(even_message.address, even_ecef, odd_ecef, interval)
else:
syncpoint = SyncPoint(even_message.address, odd_ecef, even_ecef, interval)
syncpoint.receivers.append([receiver, tA, tB, False])
if not syncpointlist:
syncpointlist = self.sync_points[key] = []
syncpointlist.append(syncpoint)
# schedule cleanup of the syncpoint after 2 seconds -
# we should have seen all copies of those messages by
# then.
asyncio.get_event_loop().call_later(
2.0,
functools.partial(self._cleanup_syncpoint,
key=key,
syncpoint=syncpoint))
def receiver_location_update(self, receiver, position_llh):
"""Note that a given receiver has moved."""
receiver.position_llh = position_llh
receiver.position = geodesy.llh2ecef(position_llh)
self._compute_interstation_distances(receiver)
