From 4be083fcfb234db69b69e74827f75715d7ed1870 Mon Sep 17 00:00:00 2001
From: Joshua Davidson <joshuadavidson2000@gmail.com>
Date: Tue, 24 Jul 2018 18:52:36 -0400
Subject: [PATCH] Control: Update FMGC altitude and heading control to V3.2

---
 Nasal/FMGC-b.nas          |  92 +++--------------------
 Systems/fmgc-drivers.xml  | 149 +++++++++++++++++++++++++++++---------
 Systems/fmgc-pitch.xml    |  52 ++++++++++---
 Systems/fmgc-roll-yaw.xml |   2 +-
 4 files changed, 169 insertions(+), 126 deletions(-)

diff --git a/Nasal/FMGC-b.nas b/Nasal/FMGC-b.nas
index 0d1c6d5e..ef9ef1e2 100644
--- a/Nasal/FMGC-b.nas
+++ b/Nasal/FMGC-b.nas
@@ -13,17 +13,14 @@ setprop("/it-autoflight/internal/heading-deg", getprop("/orientation/heading-mag
 setprop("/it-autoflight/internal/track-deg", getprop("/orientation/track-magnetic-deg"));
 setprop("/it-autoflight/internal/vert-speed-fpm", 0);
 setprop("/it-autoflight/internal/heading-error-deg", 0);
-setprop("/it-autoflight/internal/heading-5-sec-ahead", 0);
-setprop("/it-autoflight/internal/altitude-5-sec-ahead", 0);
+setprop("/it-autoflight/internal/heading-predicted", 0);
+setprop("/it-autoflight/internal/altitude-predicted", 0);
 setprop("/it-autoflight/internal/lnav-advance-nm", 1);
 
 setlistener("/sim/signals/fdm-initialized", func {
 	var trueSpeedKts = getprop("/instrumentation/airspeed-indicator/true-speed-kt");
 	var locdefl = getprop("/instrumentation/nav[0]/heading-needle-deflection-norm");
 	var signal = getprop("/instrumentation/nav[0]/gs-needle-deflection-norm");
-	var VS = getprop("/velocities/vertical-speed-fps");
-	var TAS = getprop("/velocities/uBody-fps");
-	var FPangle = 0;
 	var gear1 = getprop("/gear/gear[1]/wow");
 	var gear2 = getprop("/gear/gear[2]/wow");
 	var gnds_mps = 0;
@@ -40,6 +37,7 @@ setlistener("/sim/signals/fdm-initialized", func {
 	var R = 0;
 	var dist_coeff = 0;
 	var turn_dist = 0;
+	var vsnow = 0;
 });
 
 var APinit = func {
@@ -293,7 +291,7 @@ var lateral = func {
 		setprop("/it-autoflight/input/lat-arm", 0);
 		setprop("/it-autoflight/output/loc-armed", 0);
 		setprop("/it-autoflight/output/appr-armed", 0);
-		var hdg5sec = math.round(getprop("/it-autoflight/internal/heading-5-sec-ahead"));
+		var hdg5sec = math.round(getprop("/it-autoflight/internal/heading-predicted"));
 		setprop("/it-autoflight/input/hdg", hdg5sec);
 		setprop("/it-autoflight/output/lat", 0);
 		setprop("/it-autoflight/mode/lat", "HDG");
@@ -369,7 +367,7 @@ var vertical = func {
 		} else {
 			setprop("/it-autoflight/mode/arm", " ");
 		}
-		var alt5sec = math.round(getprop("/it-autoflight/internal/altitude-5-sec-ahead"), 500);
+		var alt5sec = math.round(getprop("/it-autoflight/internal/altitude-predicted"), 100);
 		setprop("/it-autoflight/input/alt", alt5sec);
 		setprop("/it-autoflight/internal/alt", alt5sec);
 		thrustmode();
@@ -456,11 +454,9 @@ var vertical = func {
 			setprop("/it-autoflight/mode/arm", " ");
 		}
 	} else if (vertset == 5) {
-		fpa_calc();
 		alandt.stop();
 		alandt1.stop();
 		mng_sys_stop();
-		fpa_calct.start();
 		setprop("/it-autoflight/output/appr-armed", 0);
 		var altinput = getprop("/it-autoflight/input/alt");
 		setprop("/it-autoflight/internal/alt", altinput);
@@ -659,18 +655,6 @@ var alt_on = func {
 	minmaxtimer.start();
 }
 
-var fpa_calc = func {
-	VS = getprop("/velocities/vertical-speed-fps");
-	TAS = getprop("/velocities/uBody-fps");
-	if (TAS < 10) TAS = 10;
-	if (VS < -200) VS =-200;
-	if (abs(VS/TAS) <= 1) {
-		FPangle = math.asin(VS/TAS);
-		FPangle *=90;
-		setprop("/it-autoflight/internal/fpa", FPangle);
-	}
-}
-
 setlistener("/it-autoflight/input/kts-mach", func {
 	var ias = getprop("/it-autoflight/input/spd-kts") or 0;
 	var mach = getprop("/it-autoflight/input/spd-mach") or 0;
@@ -746,59 +730,29 @@ var updateTimers = func {
 		var vertm = getprop("/it-autoflight/output/vert");
 		if (vertm == 1) {
 			altcaptt.start();
-			fpa_calct.stop();
 		} else if (vertm == 4) {
 			altcaptt.start();
-			fpa_calct.stop();
 		} else if (vertm == 5) {
 			altcaptt.start();
 		} else if (vertm == 7) {
 			altcaptt.start();
-			fpa_calct.stop();
 		} else if (vertm == 8) {
 			altcaptt.stop();
-			fpa_calct.stop();
 		} else if (vertm == 9) {
 			altcaptt.start();
-			fpa_calct.stop();
 		} else {
 			altcaptt.stop();
-			fpa_calct.stop();
 		}
 	} else {
 		altcaptt.start();
-		fpa_calct.stop();
 	}
 }
 
 # Altitude Capture
 var altcapt = func {
-	var vsnow = getprop("/it-autoflight/internal/vert-speed-fpm");
-	if ((vsnow >= 0 and vsnow < 500) or (vsnow < 0 and vsnow > -500)) {
-		setprop("/it-autoflight/internal/captvs", 100);
-		setprop("/it-autoflight/internal/captvsneg", -100);
-	} else  if ((vsnow >= 500 and vsnow < 1000) or (vsnow < -500 and vsnow > -1000)) {
-		setprop("/it-autoflight/internal/captvs", 200);
-		setprop("/it-autoflight/internal/captvsneg", -200);
-	} else  if ((vsnow >= 1000 and vsnow < 1500) or (vsnow < -1000 and vsnow > -1500)) {
-		setprop("/it-autoflight/internal/captvs", 300);
-		setprop("/it-autoflight/internal/captvsneg", -300);
-	} else  if ((vsnow >= 1500 and vsnow < 2000) or (vsnow < -1500 and vsnow > -2000)) {
-		setprop("/it-autoflight/internal/captvs", 400);
-		setprop("/it-autoflight/internal/captvsneg", -400);
-	} else  if ((vsnow >= 2000 and vsnow < 3000) or (vsnow < -2000 and vsnow > -3000)) {
-		setprop("/it-autoflight/internal/captvs", 600);
-		setprop("/it-autoflight/internal/captvsneg", -600);
-	} else  if ((vsnow >= 3000 and vsnow < 4000) or (vsnow < -3000 and vsnow > -4000)) {
-		setprop("/it-autoflight/internal/captvs", 900);
-		setprop("/it-autoflight/internal/captvsneg", -900);
-	} else  if ((vsnow >= 4000 and vsnow < 5000) or (vsnow < -4000 and vsnow > -5000)) {
-		setprop("/it-autoflight/internal/captvs", 1200);
-		setprop("/it-autoflight/internal/captvsneg", -1200);
-	} else  if ((vsnow >= 5000) or (vsnow < -5000)) {
-		setprop("/it-autoflight/internal/captvs", 1500);
-		setprop("/it-autoflight/internal/captvsneg", -1500);
-	}
+	vsnow = getprop("/it-autoflight/internal/vert-speed-fpm");
+	setprop("/it-autoflight/internal/captvs", math.round(vsnow / 5, 50));
+	setprop("/it-autoflight/internal/captvsneg", -1 * math.round(vsnow / 5, 50));
 	if ((getprop("/it-autoflight/output/fd1") == 1 or getprop("/it-autoflight/output/fd2") == 1 or getprop("/it-autoflight/output/ap1") == 1 or getprop("/it-autoflight/output/ap2") == 1) and getprop("/it-autoflight/output/vert") != 9) {
 		var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
 		var alt = getprop("/it-autoflight/internal/alt");
@@ -1216,32 +1170,9 @@ var mng_des_fpm = func {
 
 # Managed Capture
 var mng_altcapt = func {
-	var vsnow = getprop("/it-autoflight/internal/vert-speed-fpm");
-	if ((vsnow >= 0 and vsnow < 500) or (vsnow < 0 and vsnow > -500)) {
-		setprop("/it-autoflight/internal/captvs", 100);
-		setprop("/it-autoflight/internal/captvsneg", -100);
-	} else  if ((vsnow >= 500 and vsnow < 1000) or (vsnow < -500 and vsnow > -1000)) {
-		setprop("/it-autoflight/internal/captvs", 200);
-		setprop("/it-autoflight/internal/captvsneg", -200);
-	} else  if ((vsnow >= 1000 and vsnow < 1500) or (vsnow < -1000 and vsnow > -1500)) {
-		setprop("/it-autoflight/internal/captvs", 300);
-		setprop("/it-autoflight/internal/captvsneg", -300);
-	} else  if ((vsnow >= 1500 and vsnow < 2000) or (vsnow < -1500 and vsnow > -2000)) {
-		setprop("/it-autoflight/internal/captvs", 400);
-		setprop("/it-autoflight/internal/captvsneg", -400);
-	} else  if ((vsnow >= 2000 and vsnow < 3000) or (vsnow < -2000 and vsnow > -3000)) {
-		setprop("/it-autoflight/internal/captvs", 600);
-		setprop("/it-autoflight/internal/captvsneg", -600);
-	} else  if ((vsnow >= 3000 and vsnow < 4000) or (vsnow < -3000 and vsnow > -4000)) {
-		setprop("/it-autoflight/internal/captvs", 900);
-		setprop("/it-autoflight/internal/captvsneg", -900);
-	} else  if ((vsnow >= 4000 and vsnow < 5000) or (vsnow < -4000 and vsnow > -5000)) {
-		setprop("/it-autoflight/internal/captvs", 1200);
-		setprop("/it-autoflight/internal/captvsneg", -1200);
-	} else  if ((vsnow >= 5000) or (vsnow < -5000)) {
-		setprop("/it-autoflight/internal/captvs", 1500);
-		setprop("/it-autoflight/internal/captvsneg", -1500);
-	}
+	vsnow = getprop("/it-autoflight/internal/vert-speed-fpm");
+	setprop("/it-autoflight/internal/captvs", math.round(vsnow / 5, 50));
+	setprop("/it-autoflight/internal/captvsneg", -1 * math.round(vsnow / 5, 50));
 	var MNGalt = getprop("/it-autoflight/internal/mng-alt");
 	var MCPalt = getprop("/it-autoflight/internal/alt");
 	var MNGdif = abs(MNGalt - MCPalt);
@@ -1325,7 +1256,6 @@ var minmaxtimer = maketimer(0.5, minmax);
 var alandt = maketimer(0.5, aland);
 var alandt1 = maketimer(0.5, aland1);
 var reduct = maketimer(0.5, toga_reduc);
-var fpa_calct = maketimer(0.1, fpa_calc);
 var ap_varioust = maketimer(1, ap_various);
 var mng_maint = maketimer(0.5, mng_main);
 var mng_altcaptt = maketimer(0.5, mng_altcapt);
diff --git a/Systems/fmgc-drivers.xml b/Systems/fmgc-drivers.xml
index db4dab87..d6a4ba14 100644
--- a/Systems/fmgc-drivers.xml
+++ b/Systems/fmgc-drivers.xml
@@ -14,41 +14,6 @@
  <!-- Predictors                                                      -->
  <!-- =============================================================== -->
  
-	<predict-simple>
-		<name>HEADING 5 SECONDS AHEAD</name>
-		<debug>false</debug>
-		<input>
-			<condition>
-				<equals>
-					<property>/it-autoflight/input/trk</property>
-					<value>0</value>
-				</equals>
-			</condition>
-			<property>/it-autoflight/internal/heading-deg</property>
-		</input>
-		<input>
-			<condition>
-				<equals>
-					<property>/it-autoflight/input/trk</property>
-					<value>1</value>
-				</equals>
-			</condition>
-			<property>/it-autoflight/internal/track-deg</property>
-		</input>
-		<output>/it-autoflight/internal/heading-5-sec-ahead</output>
-		<seconds>5.0</seconds>
-		<filter-gain>0.0</filter-gain>
-	</predict-simple>
-
-	<predict-simple>
-		<name>ALTITUDE 5 SECONDS AHEAD</name>
-		<debug>false</debug>
-		<input>/instrumentation/altimeter/indicated-altitude-ft</input>
-		<output>/it-autoflight/internal/altitude-5-sec-ahead</output>
-		<seconds>5.0</seconds>
-		<filter-gain>0.0</filter-gain>
-	</predict-simple>
-	
 	<predict-simple>
 		<name>IAS 5 SECOND PREDICTOR</name>
 		<debug>false</debug>
@@ -702,6 +667,120 @@
 		<max>0</max>
 	</filter>
 	
+	<filter>
+		<name>HEADING PREDICTOR STAGE 1</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<gain>1.0</gain>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/trk</property>
+					<value>0</value>
+				</equals>
+			</condition>
+			<expression>
+				<sum>
+					<div>
+						<property>/orientation/roll-deg</property>
+						<value>1.8</value>
+					</div>
+					<property>/it-autoflight/internal/heading-deg</property>
+				</sum>
+			</expression>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/trk</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<expression>
+				<sum>
+					<div>
+						<property>/orientation/roll-deg</property>
+						<value>1.8</value>
+					</div>
+					<property>/it-autoflight/internal/track-deg</property>
+				</sum>
+			</expression>
+		</input>
+		<output>/it-autoflight/internal/heading-predicted-cmd</output>
+	</filter>
+	
+	<filter>
+		<name>HEADING PREDICTOR STAGE 2</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<gain>1.0</gain>
+		<input>
+			<condition>
+				<greater-than>
+					<property>/it-autoflight/internal/heading-predicted-cmd</property>
+					<value>360</value>
+				</greater-than>
+			</condition>
+			<expression>
+				<dif>
+					<property>/it-autoflight/internal/heading-predicted-cmd</property>
+					<value>360</value>
+				</dif>
+			</expression>
+		</input>
+		<input>
+			<condition>
+				<less-than>
+					<property>/it-autoflight/internal/heading-predicted-cmd</property>
+					<value>1</value>
+				</less-than>
+			</condition>
+			<expression>
+				<sum>
+					<property>/it-autoflight/internal/heading-predicted-cmd</property>
+					<value>360</value>
+				</sum>
+			</expression>
+		</input>
+		<input>/it-autoflight/internal/heading-predicted-cmd</input>
+		<output>/it-autoflight/internal/heading-predicted</output>
+	</filter>
+	
+	<filter>
+		<name>ALTITUDE PREDICTOR</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<gain>1.0</gain>
+		<input>
+			<expression>
+				<sum>
+					<div>
+						<property>/it-autoflight/internal/vert-speed-fpm</property>
+						<value>5</value>
+					</div>
+					<property>/instrumentation/altimeter/indicated-altitude-ft</property>
+				</sum>
+			</expression>
+		</input>
+		<output>/it-autoflight/internal/altitude-predicted</output>
+	</filter>
+	
+	<filter>
+		<name>INTERNAL FPA COMPUTER</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<gain>1.0</gain>
+		<input>
+			<expression>
+				<dif>
+					<property>/orientation/pitch-deg</property>
+					<property>/orientation/alpha-deg</property>
+				</dif>
+			</expression>
+		</input>
+		<output>/it-autoflight/internal/fpa</output>
+	</filter>
+
  <!-- =============================================================== -->
  <!-- Flight Director                                                 -->
  <!-- =============================================================== -->
diff --git a/Systems/fmgc-pitch.xml b/Systems/fmgc-pitch.xml
index 9548e096..acfbe56b 100644
--- a/Systems/fmgc-pitch.xml
+++ b/Systems/fmgc-pitch.xml
@@ -18,10 +18,11 @@
 		<name>ALTITUDE CAPTURE/HOLD</name>
 		<debug>false</debug>
 		<type>gain</type>
+		<gain>-5</gain>
 		<input>
 			<expression>
 				<sum>
-					<property>/it-autoflight/internal/altitude-5-sec-ahead</property>
+					<property>/instrumentation/altimeter/indicated-altitude-ft</property>
 					<value>-2</value>
 				</sum>
 			</expression>
@@ -44,7 +45,6 @@
 				</condition>
 			<property>/it-autoflight/internal/mng-alt</property>
 		</reference>
-		<gain>-8</gain>
 		<output>
 			<property>/it-autoflight/internal/target-fpm</property>
 		</output>
@@ -97,12 +97,20 @@
 			<property>/it-autoflight/internal/target-fpm-d</property>
 		</output>
 		<config>
-			<Kp>120</Kp>
+			<Kp>
+				<expression>
+					<table>
+						<property>/velocities/mach</property>
+						<entry><ind>0.2</ind><dep>120</dep></entry>
+						<entry><ind>0.9</ind><dep>340</dep></entry>
+					</table>
+				</expression>
+			</Kp>
 			<beta>1.0</beta>
 			<alpha>0.1</alpha>
 			<gamma>0.0</gamma>
-			<Ti>1.0</Ti>
-			<Td>0.00001</Td>
+			<Ti>2.0</Ti>
+			<Td>0.002</Td>
 			<u_min>
 				<expression>
 					<dif>
@@ -563,7 +571,13 @@
 						<value>6</value>
 					</not-equals>
 				</condition>
-				<value>0.0017</value>
+				<expression>
+					<table>
+						<property>/velocities/mach</property>
+						<entry><ind>0.2</ind><dep>0.0029</dep></entry>
+						<entry><ind>0.9</ind><dep>0.0010</dep></entry>
+					</table>
+				</expression>
 			</Kp>
 			<Kp>
 				<condition>
@@ -574,7 +588,7 @@
 				</condition>
 				<value>0.0055</value>
 			</Kp>
-			<Ti>3.0</Ti>
+			<Ti>2.5</Ti>
 			<Td>0.0001</Td>
 			<u_min>
 				<condition>
@@ -583,7 +597,17 @@
 						<value>6</value>
 					</not-equals>
 				</condition>
-				<value>-10</value>
+				<expression>
+					<dif>
+						<table>
+							<property>/orientation/pitch-deg</property>
+							<entry><ind>-10</ind><dep>-5</dep></entry>
+							<entry><ind>  0</ind><dep> 0</dep></entry>
+							<entry><ind> 30</ind><dep>35</dep></entry>
+						</table>
+						<value>5</value>
+					</dif>
+				</expression>
 			</u_min>
 			<u_min>
 				<condition>
@@ -607,7 +631,17 @@
 						</equals>
 					</and>
 				</condition>
-				<value>30</value>
+				<expression>
+					<sum>
+						<table>
+							<property>/orientation/pitch-deg</property>
+							<entry><ind>-10</ind><dep>-15</dep></entry>
+							<entry><ind>  0</ind><dep>  0</dep></entry>
+							<entry><ind> 30</ind><dep> 25</dep></entry>
+						</table>
+						<value>5</value>
+					</sum>
+				</expression>
 			</u_max>
 			<u_max>
 				<condition>
diff --git a/Systems/fmgc-roll-yaw.xml b/Systems/fmgc-roll-yaw.xml
index 94d47db9..662a5d59 100644
--- a/Systems/fmgc-roll-yaw.xml
+++ b/Systems/fmgc-roll-yaw.xml
@@ -61,7 +61,7 @@
 			<property>/it-autoflight/internal/target-roll-deg</property>
 		</output>
 		<config>
-			<Kp>-1.3</Kp>
+			<Kp>-1.8</Kp>
 			<Ki>-0.000001</Ki>
 			<min>
 				<property>/it-autoflight/internal/bank-limit</property>