def _do_sync(self, address, posA, posB, r0, t0A, t0B, r1, t1A, t1B):
# find or create clock pair
k = (r0, r1)
pairing = self.clock_pairs.get(k)
if pairing is None:
self.clock_pairs[k] = pairing = clocksync.ClockPairing(r0, r1)
# propagation delays, in clock units
delay0A = geodesy.ecef_distance(
posA, r0.position) * r0.clock.freq / constants.Cair
delay0B = geodesy.ecef_distance(
posB, r0.position) * r0.clock.freq / constants.Cair
delay1A = geodesy.ecef_distance(
posA, r1.position) * r1.clock.freq / constants.Cair
delay1B = geodesy.ecef_distance(
posB, r1.position) * r1.clock.freq / constants.Cair
# compute intervals, adjusted for transmitter motion
i0 = (t0B - delay0B) - (t0A - delay0A)
i1 = (t1B - delay1B) - (t1A - delay1A)
if not pairing.is_new(t0B - delay0B):
return True # timestamp is in the past or duplicated, don't use this
# do the update
return pairing.update(address, t0B - delay0B, t1B - delay1B, i0, i1)
def _add_to_existing_syncpoint(self, syncpoint, r0, t0A, t0B):
# add a new receiver and timestamps to an existing syncpoint
# new state for the syncpoint: receiver, timestamp A, timestamp B,
# and a flag indicating if this receiver actually managed to sync
# with another receiver using this syncpoint (used for stats)
receiverDistA = geodesy.ecef_distance(syncpoint.posA, r0.position)
receiverDistB = geodesy.ecef_distance(syncpoint.posB, r0.position)
# add receiver distance check here
if receiverDistA > config.MAX_RANGE or receiverDistB > config.MAX_RANGE:
r0.sync_range_exceeded = 1
return
r0.sync_range_exceeded = 0
# propagation delays, in clock units
delay0A = receiverDistA * r0.clock.freq / constants.Cair
delay0B = receiverDistB * r0.clock.freq / constants.Cair
td0A = t0A - delay0A
td0B = t0B - delay0B
# compute interval, adjusted for transmitter motion
i0 = td0B - td0A
r0l = [r0, td0B, i0, False]
# try to sync the new receiver with all receivers that previously
# saw the same pair
for r1l in syncpoint.receivers:
r1, td1B, i1, r1sync = r1l
if r1.dead:
# receiver went away before we started resolving this
continue
if r0 is r1:
# odd, but could happen
continue
now = time.monotonic()
# order the clockpair so that the receiver that sorts lower is the base clock
if r0 < r1:
if self._do_sync(syncpoint.address, now, r0, td0B, i0, r1,
td1B, i1):
# sync worked, note it for stats
r0l[3] = r1l[3] = True
else:
if self._do_sync(syncpoint.address, now, r1, td1B, i1, r0,
td0B, i0):
# sync worked, note it for stats
r0l[3] = r1l[3] = True
# update syncpoint with the new receiver and we're done
syncpoint.receivers.append(r0l)
def _compute_interstation_distances(self, receiver):
"""compute inter-station distances for a receiver"""
for other_receiver in self.receivers.values():
if other_receiver is receiver:
distance = 0
else:
distance = geodesy.ecef_distance(receiver.position, other_receiver.position)
receiver.distance[other_receiver] = distance
other_receiver.distance[receiver] = distance
def _compute_interstation_distances(self, receiver):
"""compute inter-station distances for a receiver"""
for other_receiver in self.receivers.values():
if other_receiver is receiver:
distance = 0
else:
distance = geodesy.ecef_distance(receiver.position, other_receiver.position)
receiver.distance[other_receiver] = distance
other_receiver.distance[receiver] = distance
def _do_sync(self, address, posA, posB, r0, t0A, t0B, r1, t1A, t1B):
# find or create clock pair
k = (r0, r1)
pairing = self.clock_pairs.get(k)
if pairing is None:
self.clock_pairs[k] = pairing = clocksync.ClockPairing(r0, r1)
# propagation delays, in clock units
delay0A = geodesy.ecef_distance(posA, r0.position) * r0.clock.freq / constants.Cair
delay0B = geodesy.ecef_distance(posB, r0.position) * r0.clock.freq / constants.Cair
delay1A = geodesy.ecef_distance(posA, r1.position) * r1.clock.freq / constants.Cair
delay1B = geodesy.ecef_distance(posB, r1.position) * r1.clock.freq / constants.Cair
# compute intervals, adjusted for transmitter motion
i0 = (t0B - delay0B) - (t0A - delay0A)
i1 = (t1B - delay1B) - (t1A - delay1A)
if not pairing.is_new(t0B - delay0B):
return True # timestamp is in the past or duplicated, don't use this
# do the update
return pairing.update(address, t0B - delay0B, t1B - delay1B, i0, i1)
def _residuals(x_guess, pseudorange_data, altitude, altitude_error):
"""Return an array of residuals for a position guess at x_guess versus
actual measurements pseudorange_data and altitude."""
(*position_guess, offset) = x_guess
res = []
# compute pseudoranges at the current guess vs. measured pseudorange
for receiver_position, pseudorange, error in pseudorange_data:
pseudorange_guess = geodesy.ecef_distance(receiver_position, position_guess) - offset
res.append((pseudorange - pseudorange_guess) / error)
# compute altitude at the current guess vs. measured altitude
if altitude is not None:
_, _, altitude_guess = geodesy.ecef2llh(position_guess)
res.append((altitude - altitude_guess) / altitude_error)
return res
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 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 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')
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
