Exemplo n.º 1
0
    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
Exemplo n.º 2
0
    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
Exemplo n.º 3
0
    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
]
Exemplo n.º 4
0
        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