Merge pull request IQTLabs#44 from ralatsdc/rl/include-message-age-in…

…-aircraft-lead-time

Include flight message age in aircraft lead time

axis-ptz/camera.py

@@ -3,6 +3,7 @@
 
 import argparse
 from datetime import datetime, timedelta
+from distutils.util import strtobool
 import errno
 import json
 from json.decoder import JSONDecodeError
@@ -84,7 +85,7 @@
 camera_longitude = None
 camera_altitude = None
 camera_lead = None
-
+include_age = strtobool(os.getenv("INCLUDE_AGE", "True"))
 
 def calculate_bearing_correction(b):
     return (b + cameraBearingCorrection) % 360
@@ -401,15 +402,23 @@ def calculateCameraPositionB(
     # Assign position and velocity of the aircraft
     a_varphi = currentPlane["lat"]  # [deg]
     a_lambda = currentPlane["lon"]  # [deg]
-    # currentPlane["latLonTime"]
+    a_time = currentPlane["latLonTime"]  # [s]
     a_h = currentPlane["altitude"]  # [m]
-    # currentPlane["altitudeTime"]
+    # currentPlane["altitudeTime"]  # Expect altitudeTime to equal latLonTime
     a_track = currentPlane["track"]  # [deg]
     a_ground_speed = currentPlane["groundSpeed"]  # [m/s]
     a_vertical_rate = currentPlane["verticalRate"]  # [m/s]
     # currentPlane["icao24"]
     # currentPlane["type"]
 
+    # Compute lead time accounting for age of message, and specified
+    # lead time
+    a_datetime = utils.convert_time(a_time)
+    if include_age:
+        a_lead = (datetime.utcnow() - a_datetime).total_seconds() + camera_lead  # [s]
+    else:
+        a_lead = camera_lead  # [s]
+
     # Assign position of the tripod
     t_varphi = camera_latitude  # [deg]
     t_lambda = camera_longitude  # [deg]
@@ -431,7 +440,7 @@ def calculateCameraPositionB(
             a_vertical_rate,
         ]
     )
-    r_ENz_a_1_t = r_ENz_a_0_t + v_ENz_a_0_t * camera_lead
+    r_ENz_a_1_t = r_ENz_a_0_t + v_ENz_a_0_t * a_lead
 
     # Compute position, at time one, and velocity, at time zero, in
     # the XYZ coordinate system of the aircraft relative to the tripod
@@ -489,7 +498,7 @@ def calculateCameraPositionA():
     global angularVelocityVertical
     global elevation
 
-    (lat, lon, alt) = utils.calc_travel_3d(currentPlane, camera_lead)
+    (lat, lon, alt) = utils.calc_travel_3d(currentPlane, camera_lead, include_age=include_age)
     distance3d = utils.coordinate_distance_3d(
         camera_latitude, camera_longitude, camera_altitude, lat, lon, alt
     )
@@ -504,7 +513,7 @@ def calculateCameraPositionA():
         distance2d, cameraAltitude=camera_altitude, airplaneAltitude=alt
     )
     (angularVelocityHorizontal, angularVelocityVertical) = utils.angular_velocity(
-        currentPlane, camera_latitude, camera_longitude, camera_altitude
+        currentPlane, camera_latitude, camera_longitude, camera_altitude, include_age=include_age
     )
     # logging.info("Angular Velocity - Horizontal: {} Vertical: {}".format(angularVelocityHorizontal, angularVelocityVertical))
     cameraTilt = elevation

axis-ptz/utils.py

@@ -237,7 +237,7 @@ def convert_time(inp_date_time):
     return out_date_time
 
 
-def calc_travel_3d(current_plane, lead_s: float):
+def calc_travel_3d(current_plane, lead_s: float, include_age=True):
     """Extrapolate the 3D position of the aircraft
 
     Arguments:
@@ -264,15 +264,16 @@ def calc_travel_3d(current_plane, lead_s: float):
     heading = current_plane["track"]
     climb_rate = current_plane["verticalRate"]
 
-    # TODO: Restore
-    # lat_lon_age = datetime.utcnow() - lat_lon_time
-    # lat_lon_age_s = lat_lon_age.total_seconds() + lead_s
-    lat_lon_age_s = lead_s
+    if include_age:
+        lat_lon_age = datetime.utcnow() - lat_lon_time
+        lat_lon_age_s = lat_lon_age.total_seconds() + lead_s
 
-    # TODO: Restore
-    # alt_age = datetime.utcnow() - altitude_time
-    # alt_age_s = alt_age.total_seconds() + lead_s
-    alt_age_s = lead_s
+        alt_age = datetime.utcnow() - altitude_time
+        alt_age_s = alt_age.total_seconds() + lead_s
+
+    else:
+        lat_lon_age_s = lead_s
+        alt_age_s = lead_s
 
     R = float(6371)  # Radius of the Earth in km
     brng = math.radians(heading)  # Bearing is 90 degrees converted to radians.
@@ -298,8 +299,8 @@ def calc_travel_3d(current_plane, lead_s: float):
     return (lat2, lon2, alt2)
 
 
-def angular_velocity(currentPlane, camera_latitude, camera_longitude, camera_altitude):
-    (lat, lon, alt) = calc_travel_3d(currentPlane, 0)
+def angular_velocity(currentPlane, camera_latitude, camera_longitude, camera_altitude, include_age=True):
+    (lat, lon, alt) = calc_travel_3d(currentPlane, 0, include_age=include_age)
     distance2d = coordinate_distance(camera_latitude, camera_longitude, lat, lon)
     bearing1 = bearingFromCoordinate(
         cameraPosition=[camera_latitude, camera_longitude],
@@ -310,7 +311,7 @@ def angular_velocity(currentPlane, camera_latitude, camera_longitude, camera_alt
         distance2d, cameraAltitude=camera_altitude, airplaneAltitude=alt
     )
 
-    (lat, lon, alt) = calc_travel_3d(currentPlane, 1)
+    (lat, lon, alt) = calc_travel_3d(currentPlane, 1, include_age=include_age)
     distance2d = coordinate_distance(camera_latitude, camera_longitude, lat, lon)
     bearing2 = bearingFromCoordinate(
         cameraPosition=[camera_latitude, camera_longitude],