feat: direct to with anticipated turn (#7114)

flybywire-aircraft-a320-neo/html_ui/Pages/VCockpit/Instruments/Airliners/FlyByWire_A320_Neo/CDU/A320_Neo_CDU_DirectToPage.js

@@ -1,20 +1,7 @@
-/*
- * A32NX
- * Copyright (C) 2020 FlyByWire Simulations and its contributors
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
+// Copyright (c) 2020, 2022 FlyByWire Simulations
+// SPDX-License-Identifier: GPL-3.0
+
+// TODO this whole thing is thales layout...
 
 class CDUDirectToPage {
     static ShowPage(mcdu, directWaypoint, wptsListIndex = 0) {
@@ -27,44 +14,56 @@ class CDUDirectToPage {
         let directWaypointCell = "";
         if (directWaypoint) {
             directWaypointCell = directWaypoint.ident;
-        } else if (mcdu.flightPlanManager.getDirectToTarget()) {
-            directWaypointCell = mcdu.flightPlanManager.getDirectToTarget().ident;
+        } else if (mcdu.flightPlanManager.getCurrentFlightPlanIndex() === FlightPlans.Temporary) {
+            mcdu.eraseTemporaryFlightPlan(() => {
+                CDUDirectToPage.ShowPage(mcdu);
+            });
+            return;
         }
         const waypointsCell = ["", "", "", "", ""];
         let iMax = 5;
         let eraseLabel = "";
-        if (directWaypoint) {
+        let eraseLine = "";
+        let insertLabel = "";
+        let insertLine = "";
+        if (mcdu.flightPlanManager.getCurrentFlightPlanIndex() === FlightPlans.Temporary) {
             iMax--;
             eraseLabel = "\xa0DIR TO[color]amber";
-            waypointsCell[4] = "{ERASE[color]amber";
+            eraseLine = "{ERASE[color]amber";
+            insertLabel = "TMPY\xa0[color]amber";
+            insertLine = "DIRECT*[color]amber";
             mcdu.onLeftInput[5] = () => {
-                SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO", "number", 0);
-                CDUDirectToPage.ShowPage(mcdu);
+                mcdu.eraseTemporaryFlightPlan(() => {
+                    CDUDirectToPage.ShowPage(mcdu);
+                });
+            };
+            mcdu.onRightInput[5] = () => {
+                mcdu.insertTemporaryFlightPlan(() => {
+                    SimVar.SetSimVarValue('K:A32NX.FCU_HDG_PUSH', 'boolean', true);
+                    CDUFlightPlanPage.ShowPage(mcdu);
+                });
             };
         }
-        // TODO create leg sequence
-        //  - IF at T-P
-        //  - CF equal to A/C track (turn anticipation)
-        //  - DF to waypoint or what about radial in/out?
-        //  - clear fp up to waypoint
-        //  - discont if waypoint not in FP
-        // TODO enable automatic sequencing
-        // TODO engage NAV mode
+
         mcdu.onLeftInput[0] = (value) => {
             if (value === FMCMainDisplay.clrValue) {
-                SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO", "number", 0);
-                CDUDirectToPage.ShowPage(mcdu, undefined, wptsListIndex);
+                mcdu.eraseTemporaryFlightPlan(() => {
+                    CDUDirectToPage.ShowPage(mcdu, undefined, wptsListIndex);
+                });
                 return;
             }
 
             mcdu.getOrSelectWaypointByIdent(value, (w) => {
                 if (w) {
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO", "number", 1);
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LAT_0", "number", SimVar.GetSimVarValue("PLANE LATITUDE", "degree latitude"));
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LONG_0", "number", SimVar.GetSimVarValue("PLANE LONGITUDE", "degree longitude"));
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LAT_1", "number", w.infos.coordinates.lat);
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LONG_1", "number", w.infos.coordinates.long);
-                    CDUDirectToPage.ShowPage(mcdu, w, wptsListIndex);
+                    mcdu.eraseTemporaryFlightPlan(() => {
+                        mcdu.ensureCurrentFlightPlanIsTemporary(() => {
+                            mcdu.activateDirectToWaypoint(w, () => {
+                                CDUDirectToPage.ShowPage(mcdu, w, wptsListIndex);
+                            });
+                        });
+                    });
+                } else {
+                    mcdu.setScratchpadMessage(NXSystemMessages.notInDatabase);
                 }
             });
         };
@@ -79,10 +78,10 @@ class CDUDirectToPage {
         };
         let i = 0;
         let cellIter = 0;
-        wptsListIndex = Math.max(wptsListIndex, mcdu.flightPlanManager.getActiveWaypointIndex());
-        const totalWaypointsCount = mcdu.flightPlanManager.getWaypointsCount() + mcdu.flightPlanManager.getArrivalWaypointsCount() + mcdu.flightPlanManager.getApproachWaypoints().length;
+        wptsListIndex = Math.max(wptsListIndex, mcdu.flightPlanManager.getActiveWaypointIndex(false, false, FlightPlans.Active));
+        const totalWaypointsCount = mcdu.flightPlanManager.getWaypointsCount(FlightPlans.Active);
         while (i < totalWaypointsCount && i + wptsListIndex < totalWaypointsCount && cellIter < iMax) {
-            const waypoint = mcdu.flightPlanManager.getWaypoint(i + wptsListIndex, NaN, true);
+            const waypoint = mcdu.flightPlanManager.getWaypoint(i + wptsListIndex, FlightPlans.Active, true);
             if (waypoint) {
                 if (waypoint.isVectors) {
                     i++;
@@ -91,12 +90,13 @@ class CDUDirectToPage {
                 waypointsCell[cellIter] = "{" + waypoint.ident + "[color]cyan";
                 if (waypointsCell[cellIter]) {
                     mcdu.onLeftInput[cellIter + 1] = () => {
-                        SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO", "number", 1);
-                        SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LAT_0", "number", SimVar.GetSimVarValue("PLANE LATITUDE", "degree latitude"));
-                        SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LONG_0", "number", SimVar.GetSimVarValue("PLANE LONGITUDE", "degree longitude"));
-                        SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LAT_1", "number", waypoint.infos.coordinates.lat);
-                        SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO_LONG_1", "number", waypoint.infos.coordinates.long);
-                        CDUDirectToPage.ShowPage(mcdu, waypoint, wptsListIndex);
+                        mcdu.eraseTemporaryFlightPlan(() => {
+                            mcdu.ensureCurrentFlightPlanIsTemporary(() => {
+                                mcdu.activateDirectToWaypoint(waypoint, () => {
+                                    CDUDirectToPage.ShowPage(mcdu, waypoint, wptsListIndex);
+                                });
+                            });
+                        });
                     };
                 }
             } else {
@@ -108,18 +108,6 @@ class CDUDirectToPage {
         if (cellIter < iMax) {
             waypointsCell[cellIter] = "--END--";
         }
-        let insertLabel = "";
-        let insertLine = "";
-        if (directWaypoint) {
-            insertLabel = "\xa0TMPY[color]amber";
-            insertLine = "DIRECT*[color]amber";
-            mcdu.onRightInput[5] = () => {
-                mcdu.activateDirectToWaypoint(directWaypoint, () => {
-                    SimVar.SetSimVarValue("L:A320_NEO_PREVIEW_DIRECT_TO", "number", 0);
-                    CDUFlightPlanPage.ShowPage(mcdu);
-                });
-            };
-        }
         let up = false;
         let down = false;
         if (wptsListIndex < totalWaypointsCount - 5) {
@@ -150,7 +138,7 @@ class CDUDirectToPage {
             ["", "RADIAL OUT\xa0"],
             [waypointsCell[3], "[ ]°[color]cyan"],
             [eraseLabel, insertLabel],
-            [waypointsCell[4], insertLine]
+            [eraseLine ? eraseLine : waypointsCell[4], insertLine]
         ]);
     }
 }

flybywire-aircraft-a320-neo/html_ui/Pages/VCockpit/Instruments/Airliners/FlyByWire_A320_Neo/FMC/A32NX_FMCMainDisplay.js

@@ -2635,6 +2635,7 @@ class FMCMainDisplay extends BaseAirliners {
         if (this.flightPlanManager.getCurrentFlightPlanIndex() === 1) {
             this.flightPlanManager.copyCurrentFlightPlanInto(0, () => {
                 this.flightPlanManager.setCurrentFlightPlanIndex(0, () => {
+                    this.flightPlanManager.getCurrentFlightPlan().updateTurningPoint();
                     SimVar.SetSimVarValue("L:FMC_FLIGHT_PLAN_IS_TEMPORARY", "number", 0);
                     SimVar.SetSimVarValue("L:MAP_SHOW_TEMPORARY_FLIGHT_PLAN", "number", 0);
                     if (this.tempFpPendingAutoTune) {

src/fmgc/src/flightplanning/FlightPlanManager.ts

@@ -73,6 +73,8 @@ export class FlightPlanManager {
 
     private _fixInfos: FixInfo[] = [];
 
+    private updateThrottler = new A32NX_Util.UpdateThrottler(2000);
+
     /**
      * Constructs an instance of the FlightPlanManager with the provided
      * parent instrument attached.
@@ -112,13 +114,13 @@ export class FlightPlanManager {
         this.__currentFlightPlanIndex = value;
     }
 
-    public update(_: number): void {
-        const tmpy = this._flightPlans[FlightPlans.Temporary];
-        if (tmpy) {
-            const wp = tmpy.getWaypoint(1);
-            if (wp?.additionalData?.dynamicPpos) {
-                wp.infos.coordinates.lat = SimVar.GetSimVarValue('PLANE LATITUDE', 'degree latitude');
-                wp.infos.coordinates.long = SimVar.GetSimVarValue('PLANE LONGITUDE', 'degree longitude');
+    public update(deltaTime: number): void {
+        if (this.updateThrottler.canUpdate(deltaTime) !== -1) {
+            const tmpy = this._flightPlans[FlightPlans.Temporary];
+            if (tmpy && this.__currentFlightPlanIndex === FlightPlans.Temporary) {
+                if (tmpy.updateTurningPoint()) {
+                    this.updateFlightPlanVersion();
+                }
             }
         }
     }
@@ -1630,20 +1632,7 @@ export class FlightPlanManager {
     public async activateDirectTo(icao: string, callback = EmptyCallback.Void): Promise<void> {
         const currentFlightPlan = this._flightPlans[this._currentFlightPlanIndex];
 
-        // TODO allow dir TO out of hold etc...
-        let waypointIndex = currentFlightPlan.waypoints.findIndex((w) => w.icao === icao);
-        if (waypointIndex === -1) {
-            // string, to the start of the flight plan, then direct to
-            const waypoint = await this._parentInstrument.facilityLoader.getFacilityRaw(icao).catch(console.error);
-            waypoint.endsInDiscontinuity = true;
-            waypoint.discontinuityCanBeCleared = true;
-            // TODO fix discontinuity
-            currentFlightPlan.addWaypoint(waypoint, currentFlightPlan.activeWaypointIndex);
-            waypointIndex = currentFlightPlan.waypoints.findIndex((w) => w.icao === icao);
-            currentFlightPlan.activeWaypointIndex = waypointIndex;
-        }
-
-        currentFlightPlan.addDirectTo(waypointIndex);
+        await currentFlightPlan.addDirectTo(icao);
 
         this.updateFlightPlanVersion().catch(console.error);
         callback();

src/fmgc/src/flightplanning/ManagedFlightPlan.ts

@@ -71,6 +71,8 @@ export class ManagedFlightPlan {
     /** The details of any direct-to procedures on this flight plan. */
     public directTo: DirectTo = new DirectTo();
 
+    private turningPointIndex = 0;
+
     /** The departure segment of the flight plan. */
     public get departure(): FlightPlanSegment {
         return this.getSegment(SegmentType.Departure);
@@ -799,19 +801,39 @@ export class ManagedFlightPlan {
     /**
      * Goes direct to the specified waypoint index in the flight plan.
      *
-     * @param index The waypoint index to go direct to.
+     * @param icao The waypoint to go direct to
      */
-    public addDirectTo(index: number): void {
+    public async addDirectTo(icao: string): Promise<void> {
         // TODO Replace with FMGC pos
         const lat = SimVar.GetSimVarValue('PLANE LATITUDE', 'degree latitude');
         const long = SimVar.GetSimVarValue('PLANE LONGITUDE', 'degree longitude');
+        const trueTrack = SimVar.GetSimVarValue('GPS GROUND TRUE TRACK', 'degree');
+
+        const fromWp = this.waypoints[this.activeWaypointIndex - 1];
+        const toWp = this.waypoints[this.activeWaypointIndex];
+        if (fromWp?.isTurningPoint && toWp?.additionalData?.legType === LegType.DF) {
+            this.removeWaypoint(this.activeWaypointIndex);
+        }
+
+        let waypointIndex = this.waypoints.findIndex((w) => w.icao === icao);
+        if (waypointIndex === -1) {
+            // string, to the start of the flight plan, then direct to
+            const waypoint = await this._parentInstrument.facilityLoader.getFacilityRaw(icao).catch(console.error);
+            waypoint.endsInDiscontinuity = true;
+            waypoint.discontinuityCanBeCleared = true;
+            this.addWaypoint(waypoint, this.activeWaypointIndex);
+            waypointIndex = this.waypoints.findIndex((w) => w.icao === icao);
+        }
+
+        const toWpt = this.waypoints[waypointIndex];
+        toWpt.additionalData.legType = LegType.DF;
+
+        const turningPoint = WaypointBuilder.fromCoordinates('T-P', new LatLongAlt(lat, long), this._parentInstrument, { legType: LegType.IF, course: trueTrack }, this.getTurningPointIcao());
 
-        const turningPoint = WaypointBuilder.fromCoordinates('T-P', new LatLongAlt(lat, long), this._parentInstrument);
-        turningPoint.additionalData.legType = LegType.IF;
         turningPoint.isTurningPoint = true;
 
-        this.addWaypoint(turningPoint, index);
-        this.activeWaypointIndex = index + 1;
+        this.addWaypoint(turningPoint, waypointIndex);
+        this.activeWaypointIndex = waypointIndex + 1;
 
         const deleteCount = this.activeWaypointIndex - 1;
 
@@ -820,6 +842,24 @@ export class ManagedFlightPlan {
         }
     }
 
+    public updateTurningPoint(): boolean {
+        const wp = this.getWaypoint(this.activeWaypointIndex - 1);
+        if (wp?.isTurningPoint) {
+            wp.infos.coordinates.lat = SimVar.GetSimVarValue('PLANE LATITUDE', 'degree latitude');
+            wp.infos.coordinates.long = SimVar.GetSimVarValue('PLANE LONGITUDE', 'degree longitude');
+            wp.additionalData.course = SimVar.GetSimVarValue('GPS GROUND TRUE TRACK', 'degree');
+            wp.icao = this.getTurningPointIcao();
+            wp.infos.icao = wp.icao;
+            return true;
+        }
+        return false;
+    }
+
+    private getTurningPointIcao(): string {
+        this.turningPointIndex = (this.turningPointIndex + 1) % 1000;
+        return `WXX    TP${this.turningPointIndex.toFixed(0).padStart(3, '0')}`
+    }
+
     /**
      * Builds a departure into the flight plan from indexes in the departure airport information.
      */

src/fmgc/src/flightplanning/WaypointBuilder.ts

@@ -38,7 +38,7 @@ export class WaypointBuilder {
    * @param instrument The base instrument instance.
    * @returns The built waypoint.
    */
-    public static fromCoordinates(ident: string, coordinates: LatLongAlt, instrument: BaseInstrument, additionalData?: Record<string, unknown>): WayPoint {
+    public static fromCoordinates(ident: string, coordinates: LatLongAlt, instrument: BaseInstrument, additionalData?: Record<string, unknown>, icao?: string): WayPoint {
         const waypoint = new WayPoint(instrument);
         waypoint.type = 'W';
 
@@ -48,7 +48,7 @@ export class WaypointBuilder {
         waypoint.ident = ident;
         waypoint.infos.ident = ident;
 
-        waypoint.icao = `W      ${ident}`;
+        waypoint.icao = icao ?? `W      ${ident}`;
         waypoint.infos.icao = waypoint.icao;
 
         waypoint.additionalData = additionalData ?? {};

src/fmgc/src/guidance/GuidanceController.ts

@@ -344,7 +344,7 @@ export class GuidanceController {
     recomputeGeometries() {
         const tas = SimVar.GetSimVarValue('AIRSPEED TRUE', 'Knots');
         const gs = SimVar.GetSimVarValue('GPS GROUND SPEED', 'Knots');
-        const trueTrack = SimVar.GetSimVarValue('GPS GROUND TRACK', 'degrees');
+        const trueTrack = SimVar.GetSimVarValue('GPS GROUND TRUE TRACK', 'degree');
 
         if (this.activeGeometry) {
             this.activeGeometry.recomputeWithParameters(

src/fmgc/src/guidance/GuidanceManager.ts

@@ -61,7 +61,7 @@ export class GuidanceManager {
                 return new TFLeg(prevLeg.fix, to, editableData, segment);
             }
 
-            return new IFLeg(to, editableData, segment);
+            return new IFLeg(to, editableData, segment, to.additionalData.course);
         }
 
         if (!from || !to) {

src/fmgc/src/guidance/lnav/TransitionPicker.ts

@@ -21,6 +21,7 @@ import { CRLeg } from '@fmgc/guidance/lnav/legs/CR';
 import { CILeg } from '@fmgc/guidance/lnav/legs/CI';
 import { AFLeg } from '@fmgc/guidance/lnav/legs/AF';
 import { DmeArcTransition } from '@fmgc/guidance/lnav/transitions/DmeArcTransition';
+import { IFLeg } from '@fmgc/guidance/lnav/legs/IF';
 
 export class TransitionPicker {
     static forLegs(from: Leg, to: Leg): Transition | null {
@@ -39,6 +40,9 @@ export class TransitionPicker {
         if (from instanceof HALeg || from instanceof HFLeg || from instanceof HMLeg) {
             return TransitionPicker.fromHX(from, to);
         }
+        if (from instanceof IFLeg) {
+            return TransitionPicker.fromIF(from, to);
+        }
         if (from instanceof RFLeg) {
             return TransitionPicker.fromRF(from, to);
         }
@@ -244,6 +248,14 @@ export class TransitionPicker {
         return null;
     }
 
+    private static fromIF(from: IFLeg, to: Leg): Transition | null {
+        if (to instanceof DFLeg) {
+            return new DirectToFixTransition(from, to);
+        }
+
+        return null;
+    }
+
     private static fromRF(from: RFLeg, to: Leg): Transition | null {
         if (to instanceof CALeg) {
             return new CourseCaptureTransition(from, to);

src/fmgc/src/guidance/lnav/legs/DF.ts

@@ -8,7 +8,7 @@ import { SegmentType } from '@fmgc/flightplanning/FlightPlanSegment';
 import { GuidanceParameters } from '@fmgc/guidance/ControlLaws';
 import { XFLeg } from '@fmgc/guidance/lnav/legs/XF';
 import { LnavConfig } from '@fmgc/guidance/LnavConfig';
-import { courseToFixDistanceToGo, courseToFixGuidance } from '@fmgc/guidance/lnav/CommonGeometry';
+import { courseToFixDistanceToGo, fixToFixGuidance } from '@fmgc/guidance/lnav/CommonGeometry';
 import { Transition } from '@fmgc/guidance/lnav/Transition';
 import { Leg } from '@fmgc/guidance/lnav/legs/Leg';
 import { bearingTo } from 'msfs-geo';
@@ -107,7 +107,7 @@ export class DFLeg extends XFLeg {
     }
 
     getGuidanceParameters(ppos: Coordinates, trueTrack: Degrees, _tas: Knots): GuidanceParameters | undefined {
-        return courseToFixGuidance(ppos, trueTrack, this.outboundCourse, this.fix.infos.coordinates);
+        return fixToFixGuidance(ppos, trueTrack, this.start, this.fix.infos.coordinates);
     }
 
     getNominalRollAngle(_gs: Knots): Degrees {