def pack(self):
# this is, in order:
# model, time (time.time() is fine), lag, position, orientation, linear vel, angular vel, linear accel, angular accel (accels unused), 0
# position is in ECEF format -- same as mlat uses. what luck!
pos = mlat.llh2ecef([self.lat, self.lon, self.alt * 0.3048]) # alt is in meters!
# get the rotation quaternion to rotate to local reference frame from lat/lon
rotquat = Quat([self.lat, self.lon])
# get the quaternion corresponding to aircraft orientation
acquat = Quat([self.hdg, 0, 0])
# rotate aircraft into ECEF frame
ecefquat = rotquat * acquat
# get it in angle/axis representation
(angle, axis) = ecefquat._get_angle_axis()
orientation = angle * axis
kts_to_ms = 0.514444444 # convert kts to m/s
vel_ms = self.vel * kts_to_ms
velvec = (vel_ms, 0, 0) # velocity vector in m/s -- is this in the local frame? looks like [0] is fwd vel,
# we'll pretend the a/c is always moving the dir it's pointing
turnvec = (0, 0, self.turnrate * (math.pi / 180.0)) # turn rates in rad/s [roll, pitch, yaw]
accelvec = (0, 0, 0)
turnaccelvec = (0, 0, 0)
self.posfmt = "!96s" + "d" + "d" + "3d" + "3f" + "3f" + "3f" + "3f" + "3f" + "I"
self.data = struct.pack(
self.posfmt,
self.model,
self.time,
self.lag,
pos[0],
pos[1],
pos[2],
orientation[0],
orientation[1],
orientation[2],
velvec[0],
velvec[1],
velvec[2],
turnvec[0],
turnvec[1],
turnvec[2],
accelvec[0],
accelvec[1],
accelvec[2],
turnaccelvec[0],
turnaccelvec[1],
turnaccelvec[2],
0,
)
return fg_header.pack(self) + self.data
def pack(self):
#this is, in order:
#model, time (time.time() is fine), lag, position, orientation, linear vel, angular vel, linear accel, angular accel (accels unused), 0
#position is in ECEF format -- same as mlat uses. what luck!
pos = mlat.llh2ecef([self.lat, self.lon, self.alt * 0.3048]) #alt is in meters!
#get the rotation quaternion to rotate to local reference frame from lat/lon
rotquat = Quat([self.lat, self.lon])
#get the quaternion corresponding to aircraft orientation
acquat = Quat([self.hdg, 0, 0])
#rotate aircraft into ECEF frame
ecefquat = rotquat * acquat
#get it in angle/axis representation
(angle, axis) = ecefquat._get_angle_axis()
orientation = angle * axis
kts_to_ms = 0.514444444 #convert kts to m/s
vel_ms = self.vel * kts_to_ms
velvec = (vel_ms,0,0) #velocity vector in m/s -- is this in the local frame? looks like [0] is fwd vel,
#we'll pretend the a/c is always moving the dir it's pointing
turnvec = (0,0,self.turnrate * (math.pi / 180.) ) #turn rates in rad/s [roll, pitch, yaw]
accelvec = (0,0,0)
turnaccelvec = (0,0,0)
self.posfmt = '!96s' + 'd' + 'd' + '3d' + '3f' + '3f' + '3f' + '3f' + '3f' + 'I'
self.data = struct.pack(self.posfmt,
self.model,
self.time,
self.lag,
pos[0], pos[1], pos[2],
orientation[0], orientation[1], orientation[2],
velvec[0], velvec[1], velvec[2],
turnvec[0], turnvec[1], turnvec[2],
accelvec[0], accelvec[1], accelvec[2],
turnaccelvec[0], turnaccelvec[1], turnaccelvec[2],
0)
return fg_header.pack(self) + self.data
if x["data"]["msgtype"] == 17 and 9 <= (x["data"]["longdata"] >> 51)
& 0x1F <= 18
]
offset_list = []
#there's probably a way to list-comprehension-ify this but it looks hard
for msg in position_reports:
data = msg["data"]
[alt, lat, lon, rng,
bearing] = sfparse.parseBDS05(data["shortdata"], data["longdata"],
data["parity"], data["ecc"])
ac_pos = [lat, lon, feet_to_meters(alt)]
rel_times = []
for time, station in zip(msg["times"], stations):
#here we get the estimated time at the aircraft when it transmitted
range_to_ac = numpy.linalg.norm(
numpy.array(mlat.llh2ecef(station)) -
numpy.array(mlat.llh2ecef(ac_pos)))
timestamp_at_ac = time - range_to_ac / mlat.c
rel_times.append(timestamp_at_ac)
offset_list.append({
"aircraft": data["shortdata"] & 0xffffff,
"times": rel_times
})
#this is a list of unique aircraft, heard by all stations, which transmitted position packets
#we do drift calcs separately for each aircraft in the set because mixing them seems to screw things up
#i haven't really sat down and figured out why that is yet
unique_aircraft = list(set([x["aircraft"] for x in offset_list]))
print "Aircraft heard for clock drift estimate: %s" % [
str("%x" % ac) for ac in unique_aircraft
]
all_heard.append({"data": station0_report["data"], "times": stamps})
#print all_heard
#ok, now let's pull out the location-bearing packets so we can find our time offset
position_reports = [x for x in all_heard if x["data"]["msgtype"] == 17 and 9 <= (x["data"]["longdata"] >> 51) & 0x1F <= 18]
offset_list = []
#there's probably a way to list-comprehension-ify this but it looks hard
for msg in position_reports:
data = msg["data"]
[alt, lat, lon, rng, bearing] = sfparse.parseBDS05(data["shortdata"], data["longdata"], data["parity"], data["ecc"])
ac_pos = [lat, lon, feet_to_meters(alt)]
rel_times = []
for time, station in zip(msg["times"], stations):
#here we get the estimated time at the aircraft when it transmitted
range_to_ac = numpy.linalg.norm(numpy.array(mlat.llh2ecef(station))-numpy.array(mlat.llh2ecef(ac_pos)))
timestamp_at_ac = time - range_to_ac / mlat.c
rel_times.append(timestamp_at_ac)
offset_list.append({"aircraft": data["shortdata"] & 0xffffff, "times": rel_times})
#this is a list of unique aircraft, heard by all stations, which transmitted position packets
#we do drift calcs separately for each aircraft in the set because mixing them seems to screw things up
#i haven't really sat down and figured out why that is yet
unique_aircraft = list(set([x["aircraft"] for x in offset_list]))
print "Aircraft heard for clock drift estimate: %s" % [str("%x" % ac) for ac in unique_aircraft]
print "Total reports used: %d over %.2f seconds" % (len(position_reports), position_reports[-1]["times"][0]-position_reports[0]["times"][0])
#get a list of reported times gathered by the unique aircraft that transmitted them
#abs_unique_times = [report["times"] for ac in unique_aircraft for report in offset_list if report["aircraft"] == ac]
#print abs_unique_times
#todo: the below can probably be done cleaner with nested list comprehensions