From 7c7f5875fc368a77147219a812ca0200a342a247 Mon Sep 17 00:00:00 2001
From: Josh Davidson <joshuadavidson2000@gmail.com>
Date: Sat, 18 Apr 2020 15:48:47 -0400
Subject: [PATCH] FMGC: Fix numerous bugs in the autopilot and heading/track
 control logic

---
 Nasal/FMGC/FMGC-b.nas    | 18 ++++++++++++++----
 Systems/fmgc-drivers.xml |  9 +++++++++
 revision.txt             |  2 +-
 3 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/Nasal/FMGC/FMGC-b.nas b/Nasal/FMGC/FMGC-b.nas
index d5340cb9..06136665 100644
--- a/Nasal/FMGC/FMGC-b.nas
+++ b/Nasal/FMGC/FMGC-b.nas
@@ -88,6 +88,7 @@ var Input = {
 	fd2: props.globals.initNode("/it-autoflight/input/fd2", 1, "BOOL"),
 	fpa: props.globals.initNode("/it-autoflight/input/fpa", 0, "DOUBLE"),
 	hdg: props.globals.initNode("/it-autoflight/input/hdg", 0, "INT"),
+	hdgCalc: 0,
 	ias: props.globals.initNode("/it-autoflight/input/spd-kts", 250, "INT"),
 	ktsMach: props.globals.initNode("/it-autoflight/input/kts-mach", 0, "BOOL"),
 	lat: props.globals.initNode("/it-autoflight/input/lat", 5, "INT"),
@@ -110,6 +111,7 @@ var Internal = {
 	bankLimit: props.globals.initNode("/it-autoflight/internal/bank-limit", 30, "INT"),
 	bankLimitAuto: 30,
 	captVS: 0,
+	driftAngle: props.globals.initNode("/it-autoflight/internal/drift-angle-deg", 0, "DOUBLE"),
 	flchActive: 0,
 	fpa: props.globals.initNode("/it-autoflight/internal/fpa", 0, "DOUBLE"),
 	hdg: props.globals.initNode("/it-autoflight/internal/heading-deg", 0, "DOUBLE"),
@@ -911,9 +913,13 @@ var ITAF = {
 		Input.trk.setBoolValue(1);
 		Custom.ndTrkSel[0].setBoolValue(1);
 		Custom.ndTrkSel[1].setBoolValue(1);
-		if (abs(Internal.hdgErrorDeg.getValue()) <= 10 and Output.lat.getValue() == 0) {
-			me.setLatMode(3);
+		Input.hdgCalc = Input.hdg.getValue() + math.round(Internal.driftAngle.getValue());
+		if (Input.hdgCalc > 360) { # It's rounded, so this is ok. Otherwise do >= 360.5
+			Input.hdgCalc = Input.hdgCalc - 360;
+		} else if (Input.hdgCalc < 1) { # It's rounded, so this is ok. Otherwise do < 0.5
+			Input.hdgCalc = Input.hdgCalc + 360;
 		}
+		Input.hdg.setValue(Input.hdgCalc);
 	},
 	trkFpaOff: func() {
 		Custom.trkFpa.setBoolValue(0);
@@ -923,9 +929,13 @@ var ITAF = {
 		Input.trk.setBoolValue(0);
 		Custom.ndTrkSel[0].setBoolValue(0);
 		Custom.ndTrkSel[1].setBoolValue(0);
-		if (abs(Internal.hdgErrorDeg.getValue()) <= 10 and Output.lat.getValue() == 0) {
-			me.setLatMode(3);
+		Input.hdgCalc = Input.hdg.getValue() - math.round(Internal.driftAngle.getValue());
+		if (Input.hdgCalc > 360) { # It's rounded, so this is ok. Otherwise do >= 360.5
+			Input.hdgCalc = Input.hdgCalc - 360;
+		} else if (Input.hdgCalc < 1) { # It's rounded, so this is ok. Otherwise do < 0.5
+			Input.hdgCalc = Input.hdgCalc + 360;
 		}
+		Input.hdg.setValue(Input.hdgCalc);
 	},
 };
 
diff --git a/Systems/fmgc-drivers.xml b/Systems/fmgc-drivers.xml
index ae7d4431..aa9f80b2 100644
--- a/Systems/fmgc-drivers.xml
+++ b/Systems/fmgc-drivers.xml
@@ -388,6 +388,15 @@
 		<name>DRIFT ANGLE</name>
 		<type>gain</type>
 		<gain>1.0</gain>
+		<input>
+			<condition>
+				<less-than-equals>
+					<property>/velocities/groundspeed-kt</property>
+					<value>1</value>
+				</less-than-equals>
+			</condition>
+			<property>/orientation/heading-magnetic-deg</property>
+		</input>
 		<input>/orientation/track-magnetic-deg</input>
 		<reference>/orientation/heading-magnetic-deg</reference>
 		<output>/it-autoflight/internal/drift-angle-deg</output>
diff --git a/revision.txt b/revision.txt
index 8fdd954d..b3935607 100644
--- a/revision.txt
+++ b/revision.txt
@@ -1 +1 @@
-22
\ No newline at end of file
+23
\ No newline at end of file