fix(fmgc): robust comparison of legs (#7140)

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

@@ -209,7 +209,7 @@ class CDUFlightPlanPage {
                     // ARINC Leg Types - R1A 610
                     switch (wp.additionalData.legType) {
                         case 1: // AF
-                            fixAnnotation = `${Math.round(wp.additionalData.rho).toString().substring(0, 2).padStart(2, '0')} ${wp.additionalData.navaidIdent.substring(0, 3)}`;
+                            fixAnnotation = `${Math.round(wp.additionalData.rho).toString().substring(0, 2).padStart(2, '0')} ${WayPoint.formatIdentFromIcao(wp.additionalData.recommendedIcao).substring(0, 3)}`;
                             break;
                         case 2: // CA
                         case 3: // CD

src/fmgc/src/flightplanning/LegsProcedure.ts

@@ -129,7 +129,7 @@ export class LegsProcedure {
               try {
                   switch (currentLeg.type) {
                   case LegType.AF:
-                      mappedLeg = this.mapArcToFix(currentLeg, this._previousFix);
+                      mappedLeg = this.mapExactFix(currentLeg);
                       break;
                   case LegType.CD:
                   case LegType.VD:
@@ -221,13 +221,21 @@ export class LegsProcedure {
                   mappedLeg.legAltitude2 = currentLeg.altitude2 * 3.28084;
                   mappedLeg.speedConstraint = currentLeg.speedRestriction;
                   mappedLeg.turnDirection = currentLeg.turnDirection;
+
+                  const recNavaid: RawVor | RawNdb | undefined = this._facilities.get(currentLeg.originIcao);
+                  const thetaMagVar = recNavaid ? Facilities.getMagVar(recNavaid.lat, recNavaid.lon) : Facilities.getMagVar(mappedLeg.infos.coordinates.lat, mappedLeg.infos.coordinates.long);
+
                   mappedLeg.additionalData.legType = currentLeg.type;
                   mappedLeg.additionalData.overfly = currentLeg.flyOver;
                   mappedLeg.additionalData.fixTypeFlags = currentLeg.fixTypeFlags;
-
                   mappedLeg.additionalData.distance = currentLeg.distanceMinutes ? undefined : currentLeg.distance / 1852;
                   mappedLeg.additionalData.distanceInMinutes = currentLeg.distanceMinutes ? currentLeg.distance : undefined;
                   mappedLeg.additionalData.course = currentLeg.trueDegrees ? currentLeg.course : A32NX_Util.magneticToTrue(currentLeg.course, Facilities.getMagVar(mappedLeg.infos.coordinates.lat, mappedLeg.infos.coordinates.long));
+                  mappedLeg.additionalData.recommendedIcao = currentLeg.originIcao.trim().length > 0 ? currentLeg.originIcao : undefined;
+                  mappedLeg.additionalData.recommendedFrequency = recNavaid ? recNavaid.freqMHz : undefined;
+                  mappedLeg.additionalData.recommendedLocation = recNavaid ? { lat: recNavaid.lat, long: recNavaid.lon } : undefined;
+                  mappedLeg.additionalData.rho = currentLeg.rho / 1852;
+                  mappedLeg.additionalData.theta = A32NX_Util.magneticToTrue(currentLeg.theta, thetaMagVar);
                   mappedLeg.additionalData.annotation = currentAnnotation;
               }
 
@@ -277,9 +285,6 @@ export class LegsProcedure {
 
       const waypoint = this.buildWaypoint(`${originIdent}${Math.trunc(legDistance * LegsProcedure.distanceNormalFactorNM)}`, coordinates);
 
-      waypoint.additionalData.vectorsCourse = course;
-      waypoint.additionalData.vectorsHeading = course;
-
       return waypoint;
   }
 
@@ -356,14 +361,10 @@ export class LegsProcedure {
   public mapHeadingToInterceptNextLeg(leg: RawProcedureLeg, prevLeg: WayPoint, nextLeg: RawProcedureLeg): WayPoint | null {
       const magVar = Facilities.getMagVar(prevLeg.infos.coordinates.lat, prevLeg.infos.coordinates.long);
       const course = leg.trueDegrees ? leg.course : A32NX_Util.magneticToTrue(leg.course, magVar);
-      const heading = leg.trueDegrees ? A32NX_Util.trueToMagnetic(leg.course, magVar) : leg.course;
 
       const coordinates = GeoMath.relativeBearingDistanceToCoords(course, 1, prevLeg.infos.coordinates);
       const waypoint = this.buildWaypoint(FixNamingScheme.courseToIntercept(course), coordinates, prevLeg.infos.magneticVariation);
 
-      waypoint.additionalData.vectorsCourse = course ?? leg.course;
-      waypoint.additionalData.vectorsHeading = heading;
-
       return waypoint;
   }
 
@@ -389,7 +390,6 @@ export class LegsProcedure {
 
       const magVar = Facilities.getMagVar(prevLeg.infos.coordinates.lat, prevLeg.infos.coordinates.long);
       const course = leg.trueDegrees ? leg.course : A32NX_Util.magneticToTrue(leg.course, magVar);
-      const heading = leg.trueDegrees ? A32NX_Util.trueToMagnetic(leg.course, magVar) : leg.course;
 
       const coordinates = Avionics.Utils.bearingDistanceToCoordinates(
           course,
@@ -398,10 +398,6 @@ export class LegsProcedure {
 
       const waypoint = this.buildWaypoint(`${this.getIdent(origin.icao)}${leg.theta}`, coordinates);
 
-      waypoint.additionalData.vectorsCourse = course;
-      waypoint.additionalData.vectorsHeading = heading;
-      waypoint.additionalData.origin = origin;
-      waypoint.additionalData.theta = leg.theta;
       waypoint.additionalData.radial = A32NX_Util.magneticToTrue(leg.theta, Facilities.getMagVar(origin.lat, origin.lon));
 
       return waypoint;
@@ -423,8 +419,6 @@ export class LegsProcedure {
       const coordinates = GeoMath.relativeBearingDistanceToCoords(course, distanceInNM, prevLeg.infos.coordinates);
       const waypoint = this.buildWaypoint(FixNamingScheme.headingUntilAltitude(altitudeFeet), coordinates, prevLeg.infos.magneticVariation);
 
-      waypoint.additionalData.vectorsCourse = course;
-      waypoint.additionalData.vectorsHeading = heading;
       waypoint.additionalData.vectorsAltitude = altitudeFeet;
 
       return waypoint;
@@ -447,9 +441,6 @@ export class LegsProcedure {
       waypoint.endsInDiscontinuity = true;
       waypoint.discontinuityCanBeCleared = false;
 
-      waypoint.additionalData.vectorsCourse = course;
-      waypoint.additionalData.vectorsHeading = heading;
-
       return waypoint;
   }
 
@@ -460,34 +451,14 @@ export class LegsProcedure {
    */
   public mapExactFix(leg: RawProcedureLeg): WayPoint {
       const facility = this._facilities.get(leg.fixIcao);
-      if (facility) {
-          return RawDataMapper.toWaypoint(facility, this._instrument);
-      }
-
-      const origin = this._facilities.get(leg.originIcao);
-      const originIdent = origin.icao.substring(7, 12).trim();
-
-      const coordinates = Avionics.Utils.bearingDistanceToCoordinates(leg.theta, leg.rho / 1852, origin.lat, origin.lon);
-      return this.buildWaypoint(`${originIdent}${Math.trunc(leg.rho / 1852)}`, coordinates);
+      return RawDataMapper.toWaypoint(facility, this._instrument);
   }
 
   public mapArcToFix(leg: RawProcedureLeg, prevLeg: WayPoint): WayPoint {
       const toFix = this._facilities.get(leg.fixIcao);
-      const navaid = this._facilities.get(leg.originIcao);
 
       const waypoint = RawDataMapper.toWaypoint(toFix, this._instrument);
 
-      const magVar = Facilities.getMagVar(waypoint.infos.coordinates.lat, waypoint.infos.coordinates.long);
-
-      const theta = leg.trueDegrees ? leg.theta : A32NX_Util.magneticToTrue(leg.theta, magVar);
-      const vectorsCourse = leg.trueDegrees ? leg.course : A32NX_Util.magneticToTrue(leg.course, magVar);
-
-      waypoint.additionalData.navaidIdent = navaid.icao.substring(7, 12);
-      waypoint.additionalData.navaid = { lat: navaid.lat, long: navaid.lon };
-      waypoint.additionalData.rho = leg.rho / 1852;
-      waypoint.additionalData.theta = theta;
-      waypoint.additionalData.vectorsCourse = vectorsCourse;
-
       return waypoint;
   }
 
@@ -503,7 +474,6 @@ export class LegsProcedure {
 
       waypoint.additionalData.radius = radius;
       waypoint.additionalData.center = arcCenterCoordinates;
-      waypoint.additionalData.turnDirection = leg.turnDirection;
 
       return waypoint;
   }

src/fmgc/src/flightplanning/ManagedFlightPlan.ts

@@ -1472,16 +1472,67 @@ export class ManagedFlightPlan {
         return plan;
     }
 
+    private legDataMatches(a: WayPoint, b: WayPoint, fields: string[]) {
+        return fields.every((field) => a.additionalData[field] === b.additionalData[field]);
+    }
+
+    private isLegDuplicate(a: WayPoint, b: WayPoint): boolean {
+        if (a.additionalData.legType === b.additionalData.legType) {
+            switch (a.additionalData.legType) {
+            case LegType.AF:
+            case LegType.CR:
+            case LegType.VR:
+                return this.legDataMatches(a, b, ['course', 'theta', 'recommendedIcao']);
+            case LegType.CA:
+            case LegType.VA:
+                return this.legDataMatches(a, b, ['course']) && a.legAltitude1 === b.legAltitude1;
+            case LegType.CD:
+            case LegType.VD:
+                return this.legDataMatches(a, b, ['course', 'distance', 'recommendedIcao']);
+            case LegType.CF:
+                return this.legDataMatches(a, b, ['course']) && a.icao === b.icao;
+            case LegType.CI:
+            case LegType.VI:
+            case LegType.VM:
+                return this.legDataMatches(a, b, ['course']);
+            case LegType.DF:
+            case LegType.IF:
+            case LegType.TF:
+                return a.icao === b.icao;
+            case LegType.FA:
+                return a.icao === b.icao && a.legAltitude1 === b.legAltitude1;
+            case LegType.FC:
+                return this.legDataMatches(a, b, ['course', 'distance']) && a.icao === b.icao;
+            case LegType.FD:
+                return this.legDataMatches(a, b, ['course', 'distance', 'recommendedIcao']) && a.icao === b.icao;
+            case LegType.FM:
+                return this.legDataMatches(a, b, ['course']) && a.icao === b.icao;
+            case LegType.HA:
+                return this.legDataMatches(a, b, ['course', 'distance', 'distanceInMinutes']) && a.icao === b.icao && a.legAltitude1 === b.legAltitude1;
+            case LegType.HF:
+            case LegType.HM:
+            case LegType.PI:
+                return this.legDataMatches(a, b, ['course', 'distance', 'distanceInMinutes']) && a.icao === b.icao;
+            case LegType.RF:
+                return this.legDataMatches(a, b, ['center', 'radius']) && a.icao === b.icao;
+            default:
+            }
+        } else if (ManagedFlightPlan.isXfLeg(a) && ManagedFlightPlan.isXfLeg(b)
+            || ManagedFlightPlan.isFxLeg(a) && ManagedFlightPlan.isFxLeg(b))
+        {
+            return a.icao === b.icao;
+        }
+
+        return false;
+    }
+
     private addWaypointAvoidingDuplicates(waypoint: WayPoint, waypointIndex: number, segment: FlightPlanSegment): void {
-        const dupWp = this.waypoints.find((wp) => wp.ident === waypoint.ident);
-        const index = this.waypoints.findIndex((wp) => wp.ident === waypoint.ident); // FIXME this should really compare icaos...
+        const index = this.waypoints.findIndex((wp) => this.isLegDuplicate(waypoint, wp));
 
         // FIXME this should collapse any legs between the old position and the newly inserted position
         const wptDist = Math.abs(index - waypointIndex);
 
-        const wptSameLegTypes = dupWp?.additionalData?.legType === waypoint.additionalData?.legType;
-
-        if (wptSameLegTypes && wptDist <= 2) {
+        if (index !== -1 && wptDist <= 2) {
             // console.log('  -------> MFP: addWaypointAvoidingDuplicates: removing duplicate waypoint ', this.getWaypoint(index).ident);
             const removedWp = this.getWaypoint(index);
             if (waypoint.legAltitudeDescription === AltitudeDescriptor.Empty && removedWp.legAltitudeDescription !== AltitudeDescriptor.Empty) {

src/fmgc/src/guidance/GuidanceManager.ts

@@ -80,7 +80,7 @@ export class GuidanceManager {
 
         if (to.additionalData) {
             if (to.additionalData.legType === LegType.AF) {
-                return new AFLeg(to, to.additionalData.navaid, to.additionalData.rho, to.additionalData.theta, to.additionalData.vectorsCourse, metadata, segment);
+                return new AFLeg(to, to.additionalData.recommendedLocation, to.additionalData.rho, to.additionalData.theta, to.additionalData.course, metadata, segment);
             }
 
             if (to.additionalData.legType === LegType.CF) {
@@ -97,7 +97,7 @@ export class GuidanceManager {
 
             // FIXME VALeg should be implemented to give proper heading guidance
             if (to.additionalData.legType === LegType.CA || to.additionalData.legType === LegType.VA) {
-                const course = to.additionalData.vectorsCourse;
+                const course = to.additionalData.course;
                 const altitude = to.additionalData.vectorsAltitude;
                 const extraLength = (from.additionalData.runwayLength ?? 0) / (2 * 1852);
 
@@ -109,18 +109,19 @@ export class GuidanceManager {
                     return null;
                 }
 
-                const course = to.additionalData.vectorsCourse;
+                const course = to.additionalData.course;
 
                 return new CILeg(course, nextLeg, metadata, segment);
             }
 
             if (to.additionalData.legType === LegType.CR) {
-                const course = to.additionalData.vectorsCourse;
-                const origin = to.additionalData.origin as RawFacility;
+                // TODO clean this whole thing up
+                const course = to.additionalData.course;
                 const radial = to.additionalData.radial;
                 const theta = to.additionalData.theta;
+                const ident = WayPoint.formatIdentFromIcao(to.additionalData.recommendedIcao);
 
-                const originObj = { coordinates: { lat: origin.lat, long: origin.lon }, ident: origin.icao.substring(7, 12).trim(), theta };
+                const originObj = { coordinates: to.additionalData.recommendedLocation, ident, theta };
 
                 return new CRLeg(course, originObj, radial, metadata, segment);
             }
@@ -139,7 +140,7 @@ export class GuidanceManager {
         }
 
         if (to.isVectors) {
-            return new VMLeg(to.additionalData.vectorsHeading, to.additionalData.vectorsCourse, metadata, segment);
+            return new VMLeg(to.additionalData.course, metadata, segment);
         }
 
         return new TFLeg(from, to, metadata, segment);

src/fmgc/src/guidance/lnav/LnavDriver.ts

@@ -243,7 +243,7 @@ export class LnavDriver implements GuidanceComponent {
                     }
 
                     // Track Angle Error
-                    const currentHeading = SimVar.GetSimVarValue('PLANE HEADING DEGREES MAGNETIC', 'Degrees');
+                    const currentHeading = SimVar.GetSimVarValue('PLANE HEADING DEGREES TRUE', 'Degrees');
                     const deltaHeading = MathUtils.diffAngle(currentHeading, heading);
 
                     // Update and take into account turn state; only guide using phi during a forced turn

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

@@ -180,6 +180,6 @@ export class CALeg extends Leg {
     }
 
     get repr(): string {
-        return `CA(${this.course.toFixed(1)}°) TO ${Math.round(this.altitude)} FT`;
+        return `CA(${this.course.toFixed(1)}T) TO ${Math.round(this.altitude)} FT`;
     }
 }

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

@@ -152,6 +152,6 @@ export class CFLeg extends XFLeg {
     }
 
     get repr(): string {
-        return `CF(${this.course.toFixed(1)}°) TO ${this.fix.ident}`;
+        return `CF(${this.course.toFixed(1)}T) TO ${this.fix.ident}`;
     }
 }

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

@@ -162,6 +162,6 @@ export class CILeg extends Leg {
     }
 
     get repr(): string {
-        return `CI(${Math.trunc(this.course)}°)`;
+        return `CI(${Math.trunc(this.course)}T)`;
     }
 }

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

@@ -154,6 +154,6 @@ export class CRLeg extends Leg {
     }
 
     get repr(): string {
-        return 'CR';
+        return `CR ${this.course}T to ${this.origin.ident}${this.origin.theta}`;
     }
 }

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

@@ -49,7 +49,7 @@ export class RFLeg extends XFLeg {
         const bearingFrom = Avionics.Utils.computeGreatCircleHeading(this.center, this.from.infos.coordinates); // -90?
         const bearingTo = Avionics.Utils.computeGreatCircleHeading(this.center, this.to.infos.coordinates); // -90?
 
-        switch (to.additionalData.turnDirection) {
+        switch (to.turnDirection) {
         case 1: // left
             this.clockwise = false;
             this.angle = Avionics.Utils.clampAngle(bearingFrom - bearingTo);

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

@@ -20,8 +20,7 @@ export class VMLeg extends Leg {
     predictedPath: PathVector[] = [];
 
     constructor(
-        public heading: DegreesMagnetic,
-        public course: DegreesTrue,
+        public heading: DegreesTrue,
         public readonly metadata: Readonly<LegMetadata>,
         segment: SegmentType,
     ) {
@@ -45,14 +44,16 @@ export class VMLeg extends Leg {
 
     getPathEndPoint(): Coordinates | undefined {
         return Avionics.Utils.bearingDistanceToCoordinates(
-            this.course,
+            this.heading,
             VM_LEG_SIZE,
             this.getPathStartPoint().lat,
             this.getPathStartPoint().long,
         );
     }
 
     recomputeWithParameters(_isActive: boolean, _tas: Knots, _gs: Knots, _ppos: Coordinates, _trueTrack: DegreesTrue) {
+        // FIXME course based on predicted wind
+
         this.predictedPath.length = 0;
         this.predictedPath.push(
             {
@@ -112,6 +113,6 @@ export class VMLeg extends Leg {
     }
 
     get repr(): string {
-        return `VM(${this.heading.toFixed(1)}°)`;
+        return `VM(${this.heading.toFixed(1)}T)`;
     }
 }