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