import numpy as np
import math
import datetime
import logging
import urllib
import urllib.parse
import urllib.request
from typing import Tuple,List, Union
from .wgs84 import WGS84
# ------------------------------------------------------------------------------
# _generate_aircraft_reference_frame
# ------------------------------------------------------------------------------
def generate_aircraft_reference_frame( north,east,down, pitch, roll, heading):
headrad = np.deg2rad( heading) # get the heading in radians
pitchrad = np.deg2rad(pitch) # get the pitch in radians
rollrad = np.deg2rad(roll) # get the roll in radians
coshead = np.cos(headrad) # get the cosine of the heading
sinhead = np.sin(headrad) # get the sine of the heading
cospitch = np.cos(pitchrad)
sinpitch = np.sin(pitchrad)
cosroll = np.cos(rollrad)
sinroll = np.sin(rollrad)
# ---- rotate true heading around down axis
northh = north * coshead + east * sinhead # rotate north around the down vector thru heading, these are still unit vectors
easth = east * coshead - north * sinhead # rotate east around the down vector thru heading, this is still a unit vector
# ---- rotate pitch around new easth axis
nose = northh * cospitch - down * sinpitch # get the aircraft nose unit vector, perpendicular
northhp = northh * sinpitch + down * cospitch # get the north axis, rorates thru heading and pitch
# ---- rotate roll around nose axis
starboard = easth * cosroll + northhp * sinroll
wheels = northhp * cosroll - easth * sinroll
return nose,starboard,wheels
#------------------------------------------------------------------------------
# IWG1
#------------------------------------------------------------------------------
[docs]class IWG1:
"""
**Instance Attributes:**
.. py:attribute:: utc
np.datetime64 representing the UTC time of the attitude record.
.. py:attribute:: latitude
The geodetic latitude of the aircraft in degrees North
.. py:attribute:: longitude
The geodetic longitude of the aircraft in degrees East.
.. py:attribute:: altitude
The altitude of the aircraft above mean sea level in meters.
.. py:attribute:: pitch
The pitch of the aircraft in degrees.
.. py:attribute:: roll
The roll of the aircraft in degrees.
.. py:attribute:: heading
The true heading of the aircraft in degrees. 0 degrees in North, 90 degrees is East, 180 is South and 270 is West.
.. py:attribute:: location
A 3 element array. The geocentric vector location of the aircraft in meters from the center of the earth. The coordinates of the vector are, x= 0, y = 1 and z = 2.
.. py:attribute:: north
A 3 element array. The geocentric unit vector representing geographic north at the aircraft location. The coordinates of the unit vector are x= 0, y = 1 and z = 2.
.. py:attribute:: east
A 3 element array. The geocentric unit vector representing geographic east at the aircraft location. The coordinates of the unit vector are x= 0, y = 1 and z = 2.
.. py:attribute:: down
A 3 element array. The geocentric unit vector representing geographic down at the aircraft location. Do not confuse this field with the :attr:`wheels` attribute which is typically
close to *down* when the plane is flying horizontally. The down unit vector is independent of the aircraft and only depends upon the shape of the Earth. The coordinates of the down unit vector are x= 0, y = 1 and z = 2.
.. py:attribute:: nose
A 3 element array. The unit vector of the x axis of the aircraft reference frame expressed in geocentric coordinates. This is known as the aircraft **nose** direction.
The coordinates of the unit vector are x= 0, y = 1 and z = 2.
.. py:attribute:: starboard
A 3 element array. The unit vector of the y axis of the aircraft reference frame expressed in geocentric coordinates. This is known as the aircraft **starboard** direction.
The coordinates of the unit vector are x= 0, y = 1 and z = 2.
.. py:attribute:: wheels
A 3 element array. The unit vector of the z axis of the aircraft reference frame expressed in geocentric coordinates. This is known as the aircraft **wheels** direction.
The coordinates of the unit vector are x= 0, y = 1 and z = 2.
"""
def __init__(self, utc : np.datetime64 = math.nan,
latitude : float = math.nan,
longitude : float = math.nan,
altitude : float = math.nan,
pitch : float = math.nan,
roll : float = math.nan,
heading : float = math.nan ):
self.utc = utc
self.latitude = latitude
self.longitude = longitude
self.altitude = altitude
self.pitch = pitch
self.roll = roll
self.heading = heading
self.location, self.north, self.east, self.down = WGS84.geodetic_to_geocentric(self.latitude, self.longitude, self.altitude)
self.nose, self.starboard, self.wheels = generate_aircraft_reference_frame( self.north, self.east, self.down, pitch, roll, heading )
#------------------------------------------------------------------------------
# class IWG1Collection
#------------------------------------------------------------------------------
[docs]class IWG1Collection:
"""
Decodes the IWG1Collection records generated by the NASA ER-2 system. The current code only extracts information required to determine aircraft location and orientation. Records with invalid
fields are rejected during the decode process. Several options are available during construction to initialize the records but by default the arrays will be empty. Use of the keyword
arguments allows the constructor to initialize the internal arrays.
**Instance Attributes:**
.. py:attribute:: utc
A 1-D array(npts) of datetime representing the UTC time of each attitude record.
.. py:attribute:: latitude
A 1-D array(npts). The geodetic latitude of the aircraft in degrees North
.. py:attribute:: longitude
A 1-D array(npts). The geodetic longitude of the aircraft in degrees East.
.. py:attribute:: altitude
A 1-D array(npts). The altitude of the aircraft above mean sea level in meters.
.. py:attribute:: pitch
A 1-D array(npts). The pitch of the aircraft in degrees.
.. py:attribute:: roll
A 1-D array(npts). The roll of the aircraft in degrees.
.. py:attribute:: heading
A 1-D array(npts). The true heading of the aircraft in degrees. 0 degrees in North, 90 degrees is East, 180 is South and 270 is West.
"""
def __init__(self, filename : str = None, listoflines : List[str] = None, url : str =None):
"""
:param filename:
If filename is not None then the constructor will read the IWG1Collection records from the given file using :meth:`~showattitude.iwg1.IWG1Collection.from_file`.
:param listoflines:
If listoflines is not None (and filename is None) then the constructor will initialize the IWG1Collection records with the
the given text lines using :meth:`~showattitude.iwg1.IWG1Collection.from_lines`.
:param url:
Ir url is not None (and filename and listoflines are None) then the constructor will initialize the IWG1Collection records with
the constents of teh given web page using :meth:`~showattitude.iwg1.IWG1Collection.from_url`
"""
self.utc = None
self.latitude = None
self.longitude = None
self.altitude = None
self.pitch = None
self.roll = None
self.heading = None
if filename is not None: self.from_file(filename)
elif listoflines is not None: self.from_lines(listoflines)
elif url is not None: self.from_url(url)
#------------------------------------------------------------------------------
# concatenate
#------------------------------------------------------------------------------
[docs] def concatenate(self, other : 'IWG1Collection'):
"""
Concatenates the given array of records onto the current array. No attempt is made to ensure the concatenated records are in sequential order".
:param other: Another instance of WIG1 that will concatenated to this instance.
"""
self.utc = np.concatenate( (self.utc, other.utc))
self.latitude = np.concatenate( (self.latitude, other.latitude))
self.longitude = np.concatenate( (self.longitude,other.longitude))
self.altitude = np.concatenate( (self.altitude, other.altitude))
self.pitch = np.concatenate( (self.pitch, other.pitch))
self.roll = np.concatenate( (self.roll, other.roll))
self.heading = np.concatenate( (self.heading, other.heading))
self.location = np.concatenate( (self.location, other.location), axis=1)
self.north = np.concatenate( (self.north, other.north), axis=1)
self.east = np.concatenate( (self.east, other.east), axis=1)
self.down = np.concatenate( (self.down, other.down), axis=1)
self.nose = np.concatenate( (self.nose, other.nose), axis=1)
self.starboard = np.concatenate( (self.starboard,other.starboard), axis=1)
self.wheels = np.concatenate( (self.wheels, other.wheels), axis=1)
#------------------------------------------------------------------------------
# from_lines
#------------------------------------------------------------------------------
[docs] def from_lines(self, listoflines: List[str]):
"""
Creates an array of records from a list of IWG1Collection text records. Any previous records are erased.
"""
lines = np.core.defchararray.split(listoflines, ',')
nlines = lines.size
self.utc = np.empty( [nlines], dtype= 'datetime64[us]')
self.latitude = np.empty( [nlines] )
self.longitude = np.empty( [nlines] )
self.altitude = np.empty( [nlines] )
self.pitch = np.empty( [nlines] )
self.roll = np.empty( [nlines] )
self.heading = np.empty( [nlines] )
for i in range(nlines):
self.utc[i] = np.datetime64( lines[i][1]+'000' ) #datetime.datetime.strptime( lines[i][1]+'000', '%Y-%m-%dT%H:%M:%S.%f') #2017-04-22T18:30:52.004
self.latitude [i] = float( lines[i][ 2] ) if len(lines[i][ 2]) > 0 else math.nan
self.longitude[i] = float( lines[i][ 3] ) if len(lines[i][ 3]) > 0 else math.nan
self.altitude [i] = float( lines[i][ 4] ) if len(lines[i][ 4]) > 0 else math.nan
self.pitch[i] = float( lines[i][16] ) if len(lines[i][16]) > 0 else math.nan
self.roll [i] = float( lines[i][17] ) if len(lines[i][17]) > 0 else math.nan
self.heading[i] = float( lines[i][13] ) if len(lines[i][13]) > 0 else math.nan
good = np.where ( np.isfinite( self.latitude) \
& np.isfinite( self.longitude) \
& np.isfinite( self.altitude) \
& np.isfinite( self.pitch) \
& np.isfinite( self.roll)\
& np.isfinite( self.heading) )
self.utc = self.utc[good]
self.latitude = self.latitude[good]
self.longitude = self.longitude[good]
self.altitude = self.altitude[good]
self.pitch = self.pitch[good]
self.roll = self.roll[good]
self.heading = self.heading[good]
# self.location, self.north, self.east, self.down = WGS84.geodetic_to_geocentric(self.latitude, self.longitude, self.altitude)
# self.nose, self.starboard, self.wheels = generate_aircraft_reference_frame( self.north, self.east, self.down, self.pitch, self.roll, self.heading )
#------------------------------------------------------------------------------
# load_file
#------------------------------------------------------------------------------
[docs] def from_file(self, filename:str):
"""
Creates an array of records from IWG1Collection records read in from a file. Any previous records are erased.
"""
try:
f = open(filename)
lines = f.readlines()
f.close()
except:
logging.warning('IWG1Collection::load_file, failed to load file %s',filename)
lines=[]
self.from_lines(lines)
#------------------------------------------------------------------------------
# from_url
#------------------------------------------------------------------------------
[docs] def from_url(self, url: str):
"""
Creates an array of records from IWG1Collection records read in from a URL. Any previous records are erased. Note that no timeouts are applied to the
URL read so this call meust be used with caution in time-critical systems.
"""
req = urllib.request.Request(url)
h = urllib.request.urlopen(req)
source = h.read()
h.close()
htmldata = source.decode('utf-8')
lines = htmldata.splitlines()
self.from_lines(lines)
#------------------------------------------------------------------------------
# interpolate_attitude
#------------------------------------------------------------------------------
[docs] def interpolate(self, utcnow : Union[datetime.datetime, np.datetime64], extrapolate:bool = False, truncate : bool = False, maxgap_seconds = None):
"""
Linearly interpolates the IWG1Collection tables and returns the IWG1 corresponding to the interplated time.
:param utcnow:
The time at which the attitude solution is required
:param extrapolate:
Optional parameter. If *True* then linearly extrapolate beyond the start and end of the collection
:param truncate:
:param maxgap_seconds:
:return:
"""
if (self.utc is None):
entry = IWG1(utc=math.nan,
latitude=math.nan,
longitude=math.nan,
altitude=math.nan,
pitch=math.nan,
roll=math.nan,
heading=math.nan)
else:
tnow = utcnow if type(utcnow) == np.datetime64 else np.datetime64( str(utcnow) )
upperindex = np.searchsorted( self.utc, tnow)
lowerindex = upperindex - 1
toolow = lowerindex < 0
toohigh = upperindex >= self.utc.size
f = None # set f to None to see if lower or upper limits set it to a numerical value
if toolow > 0:
if (extrapolate):
upperindex = 1
lowerindex = 0
elif truncate:
upperindex = 0
lowerindex = 0
f = 0.0
else:
upperindex = 0
lowerindex = 0
f = math.nan
if toohigh > 0:
if (extrapolate):
upperindex = self.utc.size - 1
lowerindex = self.utc.size - 2
elif truncate:
upperindex = self.utc.size - 1
lowerindex = self.utc.size - 1
f = 1.0
else:
upperindex = self.utc.size - 1
lowerindex = self.utc.size - 1
f = math.nan
if f is None:
t0 = self.utc[lowerindex]
t1 = self.utc[upperindex]
dt = t1-t0
f = (tnow - t0)/dt
if ( maxgap_seconds is not None):
maxdelta = np.timedelta64( int(maxgap_seconds*1.0E6), 'us')
if (dt > maxdelta):
f = math.nan
f1 = 1.0 - f
entry = IWG1(utc = tnow,
latitude = f1*self.latitude[lowerindex] + f*self.latitude[upperindex],
longitude = f1*self.longitude[lowerindex] + f*self.longitude[upperindex],
altitude = f1*self.altitude[lowerindex] + f*self.altitude[upperindex],
pitch = f1*self.pitch[lowerindex] + f*self.pitch[upperindex],
roll = f1*self.roll[lowerindex] + f*self.roll[upperindex],
heading = f1*self.latitude[lowerindex] + f*self.heading[upperindex])
return entry
#------------------------------------------------------------------------------
# at
#------------------------------------------------------------------------------
[docs] def at(self, index):
"""
Returns an IWG1 correposnding to the given entry.
:param index:
:return:
"""
entry = IWG1(utc = self.utc[index],
latitude = self.latitude[index],
longitude = self.longitude[index],
altitude = self.altitude[index],
pitch = self.pitch[index],
roll = self.roll[index],
heading = self.heading[index])
return entry
#------------------------------------------------------------------------------
# IWG1Collection::__len__
#------------------------------------------------------------------------------
def __len__(self):
return len(self.utc) if self.utc is not None else 0
#------------------------------------------------------------------------------
# IWG1Collection:: __getitem__(self, key)
#------------------------------------------------------------------------------
def __getitem__(self, key):
return self.at(key)
# ------------------------------------------------------------------------------
# IWG1Collection __iter__
# Create an iterator object for the collection of images
# ------------------------------------------------------------------------------
def __iter__(self):
class iterobject:
def __init__(self, iwg1coll):
self.index = -1
self.iwg1coll = iwg1coll
self.maxpts = len(iwg1coll)
def __next__(self):
self.index += 1
if (self.index >= self.maxpts): raise StopIteration
return self.iwg1coll.at(self.index)
return iterobject(self)