calculate the descend profiles based on the arrival constraints

aman/types/Inbound.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 
 import pytz
+import sys
 
 from datetime import datetime, timedelta
 
@@ -62,50 +63,106 @@ class Inbound:
             return timedelta(seconds = 0)
 
         # calculate remaining trackmiles
-        remainingDistanceNM = self.Report.distanceToIAF
+        trackmiles = self.Report.distanceToIAF
         start = self.PlannedStar.Route[0]
-        for i in range(1, len(self.PlannedStar.Route)):
-            remainingDistanceNM += start.haversine(self.PlannedStar.Route[i]) * 0.539957
+        turnIndices = [ -1, -1 ]
+        constraints = []
+        for i in range(0, len(self.PlannedStar.Route)):
+            # identified the base turn
+            if True == self.PlannedStar.Route[i].BaseTurn:
+                turnIndices[0] = i
+            # identified the final turn
+            elif -1 != turnIndices[0] and True == self.PlannedStar.Route[i].FinalTurn:
+                turnIndices[1] = i
+            # skip waypoints until the final turn point is found
+            elif -1 != turnIndices[0] and -1 == turnIndices[1]:
+                continue
+
+            trackmiles += start.haversine(self.PlannedStar.Route[i]) * 0.539957
+
+            # check if a new constraint is defined
+            altitude = -1
+            speed = -1
+            if None != self.PlannedStar.Route[i].Altitude:
+                altitude = self.PlannedStar.Route[i].Altitude
+            if None != self.PlannedStar.Route[i].Speed:
+                speed = self.PlannedStar.Route[i].Speed
+            if -1 != altitude or -1 != speed:
+                constraints.append([ trackmiles, altitude, speed ])
+
             start = self.PlannedStar.Route[i]
 
+        # add the remaining distance from the last waypoint to the runway threshold
+        trackmiles += start.haversine(self.PlannedRunway.Runway.Start)
+
+        if turnIndices[0] > turnIndices[1] or (-1 == turnIndices[1] and -1 != turnIndices[0]):
+            sys.stderr.write('Invalid constraint definition found for ' + self.PlannedStar.Name)
+            sys.exit(-1)
+
         # calculate descend profile
-        flightTimeSeconds = 0
         currentHeading = Waypoint(latitude = self.Report.position.latitude, longitude = self.Report.position.longitude).bearing(self.PlannedStar.Route[0])
+        currentIAS = self.PerformanceData.ias(self.Report.dynamics.altitude, trackmiles)
+        currentPosition = [ self.Report.dynamics.altitude, self.Report.dynamics.groundSpeed ]
         distanceToWaypoint = self.Report.distanceToIAF
+        flightTimeSeconds = 0
         nextWaypointIndex = 0
-        currentPosition = [ self.Report.dynamics.altitude, self.Report.dynamics.groundSpeed ]
-        while 0 < currentPosition[0]:
-            lastDistance = remainingDistanceNM
-
-            # TODO integrate speed and altitude constraints
-
-            # calculate the next position after 10 seconds
-            if (currentPosition[0] / 1000 * 3) > remainingDistanceNM:
+        flownDistance = 0.0
+
+        while True:
+            # check if a constraint cleanup is needed and if a speed-update is needed
+            if 0 != len(constraints) and flownDistance >= constraints[0][0]:
+                if -1 != constraints[0][2]:
+                    currentIAS = min(constraints[0][2], self.PerformanceData.ias(self.Report.dynamics.altitude, trackmiles - flownDistance))
+                    currentPosition[1] = min(weather.calculateGS(currentPosition[0], currentIAS, currentHeading), currentPosition[1])
+                constraints.pop(0)
+
+            # search next altitude constraint
+            altitudeDistance = 0
+            nextAltitude = 0
+            for constraint in constraints:
+                if -1 != constraint[1]:
+                    altitudeDistance = constraint[0]
+                    nextAltitude = constraint[1]
+                    break
+
+            # check if update of altitude and speed is needed on 3° glide
+            if currentPosition[0] > nextAltitude and ((currentPosition[0] - nextAltitude) / 1000 * 3) > (altitudeDistance - flownDistance):
                 oldGroundspeed = currentPosition[1]
                 descendRate = (currentPosition[1] / 60) / 3 * 1000 / 6
                 newAltitude = currentPosition[0] - descendRate
                 if 0 > newAltitude:
                     newAltitude = 0
 
-                # get the planned IAS and calculate the new altitude and GS out of the predicted information
-                # we assume that the aircraft only decelerates
-                ias = self.PerformanceData.ias(newAltitude, remainingDistanceNM)
-                currentPosition = [ newAltitude, min(weather.calculateGS(newAltitude, int(ias), currentHeading), currentPosition[1]) ]
-
-                # use the average between last and current speed to estimate the remaining distance
-                remainingDistanceNM -= (currentPosition[1] + oldGroundspeed) / 2 / 60 / 6
+                currentPosition = [ newAltitude, min(weather.calculateGS(newAltitude, currentIAS, currentHeading), currentPosition[1]) ]
+                distance = (currentPosition[1] + oldGroundspeed) / 2 / 60 / 6
             else:
-                remainingDistanceNM -= currentPosition[1] / 60 / 6
+                distance = currentPosition[1] / 60 / 6
+
+            # update the statistics
+            distanceToWaypoint -= distance
+            flownDistance += distance
+            newIAS = min(currentIAS, self.PerformanceData.ias(currentPosition[0], trackmiles - flownDistance))
+            if newIAS < currentIAS:
+                currentPosition[1] = min(weather.calculateGS(currentPosition[0], newIAS, currentHeading), currentPosition[1])
+                currentIAS = newIAS
 
             flightTimeSeconds += 10
-            if 0 > remainingDistanceNM:
+            if flownDistance >= trackmiles:
                 break
 
             # check if we follow a new waypoint pair
-            distanceToWaypoint -= abs(lastDistance - remainingDistanceNM)
             if 0 >= distanceToWaypoint:
+                lastWaypointIndex = nextWaypointIndex
                 nextWaypointIndex += 1
+
+                # check if a skip from base to final turn waypoints is needed
+                if -1 != turnIndices[0] and nextWaypointIndex > turnIndices[0] and nextWaypointIndex < turnIndices[1]:
+                    nextWaypointIndex = turnIndices[1]
+
+                # update the statistics
                 if nextWaypointIndex < len(self.PlannedStar.Route):
-                    currentHeading = self.PlannedStar.Route[nextWaypointIndex - 1].bearing(self.PlannedStar.Route[nextWaypointIndex])
+                    distanceToWaypoint = self.PlannedStar.Route[lastWaypointIndex].haversine(self.PlannedStar.Route[nextWaypointIndex]) * 0.539957
+                    currentHeading = self.PlannedStar.Route[lastWaypointIndex].bearing(self.PlannedStar.Route[nextWaypointIndex])
+                    currentPosition[1] = min(weather.calculateGS(newAltitude, currentIAS, currentHeading), currentPosition[1])
 
         return timedelta(seconds = flightTimeSeconds)