From 4039086128e8cf6b8e2d0f51a87f9d0839c2698f Mon Sep 17 00:00:00 2001
From: Joshua Davidson <joshuadavidson2000@gmail.com>
Date: Mon, 27 Mar 2017 16:18:28 -0400
Subject: [PATCH] ITAF 165 + all bugfixes + PID update, adjust Autothrust
 throttle speed for smoother FLCH function, more PID tuning

---
 A320-main.xml                         |  11 +-
 Models/FlightDeck/a320.flightdeck.xml |  42 +-
 Nasal/it-autoflight.nas               | 395 ++++++++++-
 Systems/autopilot-dlg.xml             |  70 +-
 Systems/custom-autothrust.xml         |  24 +-
 Systems/it-autoflight.xml             | 899 ++++++++++++++++----------
 Systems/it-autothrust.xml             | 421 ++++++------
 7 files changed, 1233 insertions(+), 629 deletions(-)

diff --git a/A320-main.xml b/A320-main.xml
index a7f0cec5..79644ffc 100644
--- a/A320-main.xml
+++ b/A320-main.xml
@@ -228,9 +228,9 @@
 	<config>
 		<altitude-dial-mode>0</altitude-dial-mode>
 		<roll>
-			<umin>-0.40</umin>
-			<umax>0.40</umax>
-			<kp>0.06</kp>
+			<umin>-0.30</umin>
+			<umax>0.30</umax>
+			<kp>0.05</kp>
 			<kp-vloc>0.09</kp-vloc>
 			<ti>10.0</ti>
 			<td>0.00001</td>
@@ -245,12 +245,13 @@
 			<kp-cws>-0.07</kp-cws>
 		</pitch>
 		<cmd>
-			<roll-scale>5</roll-scale>
+			<roll-kp>-1.9</roll-kp>
+			<roll-scale>1.4</roll-scale>
 			<vorloc>-4.0</vorloc>
 			<alt>0.025</alt>
 			<vs>0.07</vs>
 			<fpa>0.30</fpa>
-			<flch>-0.35</flch>
+			<flch>-4.0</flch>
 			<gs>0.10</gs>
 		</cmd>
 		<autoland>
diff --git a/Models/FlightDeck/a320.flightdeck.xml b/Models/FlightDeck/a320.flightdeck.xml
index d98c5fc9..0cafe56b 100644
--- a/Models/FlightDeck/a320.flightdeck.xml
+++ b/Models/FlightDeck/a320.flightdeck.xml
@@ -40,17 +40,6 @@
 		</offsets>
 	</model>
 	
-	<!-- Standby Attitude Indicator (new) -->
-	<model>
-		<path>Aircraft/A320Family/Models/Instruments/Stby-AI/Stby-AInew.xml</path>
-		<offsets>
-			<x-m>-0.5700</x-m>
-			<y-m>-0.1344</y-m>
-			<z-m>-0.0693</z-m>
-			<pitch-deg>-16</pitch-deg>
-		</offsets>
-	</model>
-	
 	<!-- Multi-purpose Control and Display Units -->
 	
 	<model>
@@ -3772,6 +3761,35 @@
 			<z2-m>0.1419</z2-m>
 		</axis>
 	</animation>
+	
+	<!-- Standby Attitude Indicator -->
+	
+	<animation>
+        <type>texmultiple</type>
+        <object-name>stby-ai</object-name>
+        <transform>
+            <property>orientation/pitch-deg</property>
+            <subtype>textranslate</subtype>
+            <factor>0.003955078</factor>
+            <axis>
+                <x>0</x>
+                <y>1</y>
+            </axis>
+        </transform>
+        <transform>
+            <property>orientation/roll-deg</property>
+            <subtype>texrotate</subtype>
+            <center>
+                <x>0.50</x>
+                <y>0.50</y>
+            </center>
+            <axis>
+                <x>0</x>
+                <y>0</y>
+                <z>-1</z>
+            </axis>
+        </transform>
+    </animation>
     
     <!-- Autobrake System -->
     
@@ -4723,4 +4741,4 @@
 		</action>
 	</animation>
 
-</PropertyList>
+</PropertyList>
\ No newline at end of file
diff --git a/Nasal/it-autoflight.nas b/Nasal/it-autoflight.nas
index a4b8bbf0..0cb83182 100644
--- a/Nasal/it-autoflight.nas
+++ b/Nasal/it-autoflight.nas
@@ -1,6 +1,6 @@
 # IT AUTOFLIGHT System Controller
 # Joshua Davidson (it0uchpods)
-# V3.0.0 Build 132
+# V3.0.0 Build 165
 # This program is 100% GPL!
 
 print("IT-AUTOFLIGHT: Please Wait!");
@@ -20,6 +20,7 @@ var ap_init = func {
 	setprop("/it-autoflight/input/lat", 5);
 	setprop("/it-autoflight/input/lat-arm", 0);
 	setprop("/it-autoflight/input/vert", 7);
+	setprop("/it-autoflight/input/prof-arm", 0);
 	setprop("/it-autoflight/input/bank-limit", 30);
 	setprop("/it-autoflight/input/trk", 0);
 	setprop("/it-autoflight/input/toga", 0);
@@ -35,6 +36,7 @@ var ap_init = func {
 	setprop("/it-autoflight/output/retard", 0);
 	setprop("/it-autoflight/output/lat", 5);
 	setprop("/it-autoflight/output/vert", 7);
+	setprop("/it-autoflight/output/prof-vert", 4);
 	setprop("/it-autoflight/settings/min-pitch", -8);
 	setprop("/it-autoflight/settings/max-pitch", 8);
 	setprop("/it-autoflight/settings/use-nav2-radio", 0);
@@ -42,18 +44,24 @@ var ap_init = func {
 	setprop("/it-autoflight/internal/min-pitch", -8);
 	setprop("/it-autoflight/internal/max-pitch", 8);
 	setprop("/it-autoflight/internal/alt", 10000);
+	setprop("/it-autoflight/internal/prof-alt", 10000);
+	setprop("/it-autoflight/internal/prof-wp-alt", 10000);
+	setprop("/it-autoflight/internal/prof-mode", "XX");
 	setprop("/it-autoflight/internal/cwsr", 0);
 	setprop("/it-autoflight/internal/cwsp", 0);
 	setprop("/it-autoflight/internal/fpa", 0);
+	setprop("/it-autoflight/internal/prof-fpm", 0);
+	setprop("/it-autoflight/internal/top-of-des-nm", 0);
 	setprop("/it-autoflight/autoland/target-vs", "-650");
 	setprop("/it-autoflight/mode/thr", "PITCH");
 	setprop("/it-autoflight/mode/arm", "HDG");
 	setprop("/it-autoflight/mode/lat", "T/O");
 	setprop("/it-autoflight/mode/vert", "T/O CLB");
+	setprop("/it-autoflight/mode/prof", "NONE");
 	setprop("/it-autoflight/input/spd-kts", 200);
 	setprop("/it-autoflight/input/spd-mach", 0.68);
-	thrustmode();
 	update_arms();
+	thrustmode();
 	print("IT-AUTOFLIGHT: Done!");
 }
 
@@ -64,7 +72,7 @@ setlistener("/it-autoflight/input/ap1", func {
 		setprop("/it-autoflight/output/ap1", 0);
 		setprop("/controls/flight/rudder", 0);
 		if (getprop("/it-autoflight/sound/enableapoffsound") == 1) {
-			setprop("/it-autoflight/sound/apoffsound", 1);	
+			setprop("/it-autoflight/sound/apoffsound", 1);
 			setprop("/it-autoflight/sound/enableapoffsound", 0);	  
 		}
 	} else if (apmas == 1) {
@@ -171,7 +179,7 @@ var lateral = func {
 		setprop("/it-autoflight/mode/lat", "HDG");
 		setprop("/it-autoflight/mode/arm", " ");
 	} else if (latset == 1) {
-		if (getprop("/autopilot/route-manager/route/num") > 0) {
+		if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
 			alandt.stop();
 			alandt1.stop();
 			lnavwptt.start();
@@ -180,12 +188,13 @@ var lateral = func {
 			setprop("/it-autoflight/output/lat", 1);
 			setprop("/it-autoflight/mode/lat", "LNAV");
 			setprop("/it-autoflight/mode/arm", " ");
+		} else {
+			gui.popupTip("Please make sure you have a route set, and that it is Activated!");
 		}
 	} else if (latset == 2) {
 		setprop("/instrumentation/nav[0]/signal-quality-norm", 0);
 		setprop("/instrumentation/nav[1]/signal-quality-norm", 0);
 		setprop("/it-autoflight/output/loc-armed", 1);
-		setprop("/it-autoflight/output/appr-armed", 0);
 		setprop("/it-autoflight/mode/arm", "LOC");
 	} else if (latset == 3) {
 		alandt.stop();
@@ -214,8 +223,12 @@ var lat_arm = func {
 		setprop("/it-autoflight/input/lat-arm", 0);
 		setprop("/it-autoflight/mode/arm", "HDG");
 	} else if (latset == 1) {
-		setprop("/it-autoflight/input/lat-arm", 1);
-		setprop("/it-autoflight/mode/arm", "LNV");
+		if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+			setprop("/it-autoflight/input/lat-arm", 1);
+			setprop("/it-autoflight/mode/arm", "LNV");
+		} else {
+			gui.popupTip("Please make sure you have a route set, and that it is Activated!");
+		}
 	} else if (latset == 3) {
 		var hdgnow = int(getprop("/orientation/heading-magnetic-deg")+0.5);
 		setprop("/it-autoflight/input/hdg", hdgnow);
@@ -228,6 +241,8 @@ var lat_arm = func {
 setlistener("/it-autoflight/input/vert", func {
 	if ((getprop("/gear/gear[1]/wow") == 0) and (getprop("/gear/gear[2]/wow") == 0)) {
 		vertical();
+	} else {
+		vert_arm();
 	}
 });
 
@@ -236,6 +251,7 @@ var vertical = func {
 	if (vertset == 0) {
 		alandt.stop();
 		alandt1.stop();
+		prof_maint.stop();
 		setprop("/it-autoflight/output/appr-armed", 0);
 		setprop("/it-autoflight/output/vert", 0);
 		setprop("/it-autoflight/mode/vert", "ALT HLD");
@@ -251,11 +267,12 @@ var vertical = func {
 	} else if (vertset == 1) {
 		alandt.stop();
 		alandt1.stop();
+		prof_maint.stop();
+		setprop("/it-autoflight/output/appr-armed", 0);
 		var altinput = getprop("/it-autoflight/input/alt");
 		setprop("/it-autoflight/internal/alt", altinput);
 		var vsnow = int(getprop("/velocities/vertical-speed-fps")*0.6)*100;
 		setprop("/it-autoflight/input/vs", vsnow);
-		setprop("/it-autoflight/output/appr-armed", 0);
 		setprop("/it-autoflight/output/vert", 1);
 		setprop("/it-autoflight/mode/vert", "V/S");
 		if (getprop("/it-autoflight/output/loc-armed")) {
@@ -280,6 +297,7 @@ var vertical = func {
 	} else if (vertset == 3) {
 		alandt.stop();
 		alandt1.stop();
+		prof_maint.stop();
 		var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
 		var alt = getprop("/it-autoflight/internal/alt");
 		var dif = calt - alt;
@@ -296,6 +314,8 @@ var vertical = func {
 	} else if (vertset == 4) {
 		alandt.stop();
 		alandt1.stop();
+		prof_maint.stop();
+		setprop("/it-autoflight/output/appr-armed", 0);
 		var altinput = getprop("/it-autoflight/input/alt");
 		setprop("/it-autoflight/internal/alt", altinput);
 		var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
@@ -314,15 +334,16 @@ var vertical = func {
 	} else if (vertset == 5) {
 		alandt.stop();
 		alandt1.stop();
+		prof_maint.stop();
 		fpa_calct.start();
+		setprop("/it-autoflight/output/appr-armed", 0);
 		var altinput = getprop("/it-autoflight/input/alt");
 		setprop("/it-autoflight/internal/alt", altinput);
 		var fpanow = (int(10*getprop("/it-autoflight/internal/fpa")))*0.1;
 		setprop("/it-autoflight/input/fpa", fpanow);
-		setprop("/it-autoflight/output/appr-armed", 0);
 		setprop("/it-autoflight/output/vert", 5);
 		setprop("/it-autoflight/mode/vert", "FPA");
-		if (getprop("/it-autoflight/output/loc-armed")) {
+		if (getprop("/it-autoflight/output/loc-armed") == 1) {
 			setprop("/it-autoflight/mode/arm", "LOC");
 		} else {
 			setprop("/it-autoflight/mode/arm", " ");
@@ -335,15 +356,53 @@ var vertical = func {
 		thrustmode();
 		alandt.stop();
 		alandt1.start();
+		prof_maint.stop();
 		setprop("/it-autoflight/autoland/target-vs", "-650");
 	} else if (vertset == 7) {
+		alandt.stop();
+		alandt1.stop();
+		prof_maint.stop();
 		setprop("/it-autoflight/output/vert", 7);
 		setprop("/it-autoflight/mode/arm", " ");
 		var altinput = getprop("/it-autoflight/input/alt");
 		setprop("/it-autoflight/internal/alt", altinput);
+		prof_maint.stop();
 		thrustmodet.start();
-		alandt.stop();
-		alandt1.stop();
+	} else if (vertset == 8) {
+		if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+			alandt.stop();
+			alandt1.stop();
+			setprop("/it-autoflight/output/appr-armed", 0);
+			setprop("/it-autoflight/output/vert", 8);
+			prof_run();
+			setprop("/it-autoflight/mode/vert", "VNAV");
+			setprop("/it-autoflight/mode/arm", " ");
+			var altinput = getprop("/it-autoflight/input/alt");
+			setprop("/it-autoflight/internal/alt", altinput);
+			if (getprop("/it-autoflight/output/loc-armed")) {
+				setprop("/it-autoflight/mode/arm", "LOC");
+			} else {
+				setprop("/it-autoflight/mode/arm", " ");
+			}
+			prof_maint.start();
+		} else {
+			gui.popupTip("Please make sure you have a route set, and that it is Activated!");
+		}
+	}
+}
+
+var vert_arm = func {
+	var vertset = getprop("/it-autoflight/input/vert");
+	if (vertset == 8) {
+		if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+			setprop("/it-autoflight/input/prof-arm", 1);
+			setprop("/it-autoflight/mode/prof", "ARMED");
+		} else {
+			gui.popupTip("Please make sure you have a route set, and that it is Activated!");
+		}
+	} else {
+		setprop("/it-autoflight/input/prof-arm", 0);
+		setprop("/it-autoflight/mode/prof", "NONE");
 	}
 }
 
@@ -369,7 +428,7 @@ var alt_on = func {
 	setprop("/it-autoflight/output/vert", 0);
 	setprop("/it-autoflight/mode/vert", "ALT CAP");
 	setprop("/it-autoflight/internal/max-pitch", 8);
-	setprop("/it-autoflight/internal/min-pitch", -8);
+	setprop("/it-autoflight/internal/min-pitch", -5);
 	minmaxtimer.start();
 }
 
@@ -432,16 +491,27 @@ setlistener("/it-autoflight/mode/vert", func {
 	var vertm = getprop("/it-autoflight/mode/vert");
 	if (vertm == "T/O CLB") {
 		reduct.start();
-		latarmt.start();
 	} else {
 		reduct.stop();
+	}
+});
+
+setlistener("/it-autoflight/mode/lat", func {
+	var vertm = getprop("/it-autoflight/mode/lat");
+	if (vertm == "T/O") {
+		latarmt.start();
+	} else {
 		latarmt.stop();
 	}
 });
 
 var toga_reduc = func {
 	if (getprop("/position/gear-agl-ft") >= getprop("/it-autoflight/settings/reduc-agl-ft")) {
-		setprop("/it-autoflight/input/vert", 4);
+		if (getprop("/it-autoflight/input/prof-arm") == 1) {
+			setprop("/it-autoflight/input/vert", 8);
+		} else {
+			setprop("/it-autoflight/input/vert", 4);
+		}
 	}
 }
 
@@ -451,17 +521,25 @@ setlistener("/it-autoflight/output/vert", func {
 	if (vertm == 1) {
 		altcaptt.start();
 		fpa_calct.stop();
+		setprop("/it-autoflight/mode/prof", "NONE");
 	} else if (vertm == 4) {
 		altcaptt.start();
 		fpa_calct.stop();
+		setprop("/it-autoflight/mode/prof", "NONE");
 	} else if (vertm == 5) {
 		altcaptt.start();
+		setprop("/it-autoflight/mode/prof", "NONE");
 	} else if (vertm == 7) {
 		altcaptt.start();
 		fpa_calct.stop();
+		setprop("/it-autoflight/mode/prof", "NONE");
+	} else if (vertm == 8) {
+		altcaptt.stop();
+		fpa_calct.stop();
 	} else {
 		altcaptt.stop();
 		fpa_calct.stop();
+		setprop("/it-autoflight/mode/prof", "NONE");
 	}
 });
 
@@ -544,7 +622,9 @@ var thrustmode = func {
 	} else if (vertm == 7) {
 		setprop("/it-autoflight/output/thr-mode", 2);
 		setprop("/it-autoflight/mode/thr", " PITCH");
-	} else {
+	} else if (vertm == 8) {
+		thrustmodet.stop();
+	}  else {
 		setprop("/it-autoflight/output/thr-mode", 0);
 		setprop("/it-autoflight/mode/thr", "THRUST");
 		thrustmodet.stop();
@@ -649,9 +729,9 @@ var make_loc_active = func {
 var apparmcheck = func {
 	var signal = getprop("/instrumentation/nav[0]/gs-needle-deflection-norm");
 	var signal_b = getprop("/instrumentation/nav[1]/gs-needle-deflection-norm");
-	if ((signal <= -0.000000001) and (getprop("/it-autoflight/settings/use-nav2-radio") == 0)) {
+	if ((signal <= -0.000000001) and (getprop("/it-autoflight/settings/use-nav2-radio") == 0) and (getprop("/it-autoflight/output/lat") == 2)) {
 		make_appr_active();
-	} else if ((signal_b <= -0.000000001) and (getprop("/it-autoflight/settings/use-nav2-radio") == 1)) {
+	} else if ((signal_b <= -0.000000001) and (getprop("/it-autoflight/settings/use-nav2-radio") == 1) and (getprop("/it-autoflight/output/lat") == 2)) {
 		make_appr_active();
 	} else {
 		return 0;
@@ -712,6 +792,276 @@ var aland1 = func {
 }
 
 # Autoland Stage 2 Logic (Rollout)
+# Not yet working, planned.
+
+# VNAV Profile Mode
+var prof_main = func {
+	if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+		var altinput = getprop("/it-autoflight/input/alt");
+		setprop("/it-autoflight/internal/alt", altinput);
+		var wp_curr = getprop("/autopilot/route-manager/current-wp");
+		var vnav_alt_wp = getprop("/autopilot/route-manager/route/wp",wp_curr,"altitude-ft");
+		if (getprop("/it-autoflight/internal/prof-wp-alt") == vnav_alt_wp) {
+			# Do nothing
+		} else {
+			setprop("/it-autoflight/internal/prof-wp-alt", vnav_alt_wp);
+		}
+		vnav_alt_selector();
+	} else {
+		setprop("/it-autoflight/input/vert", 4);
+	}
+}
+
+setlistener("/it-autoflight/input/alt", func {
+	if (getprop("/it-autoflight/output/vert") == 8) {
+		vnav_alt_selector();
+		prof_run();
+	}
+});
+
+setlistener("/it-autoflight/internal/prof-wp-alt", func {
+	if (getprop("/it-autoflight/output/vert") == 8) {
+		vnav_alt_selector();
+		prof_run();
+	}
+});
+
+setlistener("/autopilot/route-manager/current-wp", func {
+	if (getprop("/it-autoflight/output/vert") == 8) {
+		vnav_alt_selector();
+		prof_run();
+	}
+});
+
+var prof_run = func {
+	if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+		var wp_curr = getprop("/autopilot/route-manager/current-wp");
+		var wptnum = getprop("/autopilot/route-manager/current-wp");
+		var vnav_alt_wp = getprop("/autopilot/route-manager/route/wp",wp_curr,"altitude-ft");
+		if ((wptnum - 1) < getprop("/autopilot/route-manager/route/num")) {
+			var vnav_alt_wp_prev = getprop("/autopilot/route-manager/route/wp",wp_curr - 1,"altitude-ft");
+			if (vnav_alt_wp_prev > vnav_alt_wp) {
+				vnav_des_todt.start();
+				setprop("/it-autoflight/internal/prof-mode", "DES");
+			} else if (vnav_alt_wp_prev == vnav_alt_wp) {
+				vnav_des_todt.stop();
+				setprop("/it-autoflight/internal/top-of-des-nm", 0);
+				setprop("/it-autoflight/internal/prof-mode", "XX");
+			} else if (vnav_alt_wp_prev <= vnav_alt_wp) {
+				vnav_des_todt.stop();
+				setprop("/it-autoflight/internal/top-of-des-nm", 0);
+				setprop("/it-autoflight/internal/prof-mode", "CLB");
+			}
+		} else {
+			vnav_des_todt.stop();
+			setprop("/it-autoflight/internal/top-of-des-nm", 0);
+		}
+		if (vnav_alt_wp > 100) {
+			if (getprop("/it-autoflight/internal/prof-mode") == "CLB") {
+				var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+				var valt = getprop("/it-autoflight/internal/prof-alt");
+				var vdif = calt - valt;
+				if (vdif > 550 or vdif < -550) {
+					prof_clb();
+				} else {
+					vnav_alt_sel();
+				}
+			} else if (getprop("/it-autoflight/internal/prof-mode") == "DES") {
+				var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+				var valt = getprop("/it-autoflight/internal/prof-alt");
+				var vdif = calt - valt;
+				if (vdif > 550 or vdif < -550) {
+					prof_des_spd();
+				} else {
+					vnav_alt_sel();
+				}
+			} else if (getprop("/it-autoflight/internal/prof-mode") == "XX") {
+				# Do nothing for now
+			}
+		} else {
+			setprop("/it-autoflight/input/vert", 4);
+		}
+	} else {
+		setprop("/it-autoflight/input/vert", 4);
+	}
+}
+
+# VNAV Top of Descent
+var vnav_des_tod = func {
+	if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+		var wp_curr = getprop("/autopilot/route-manager/current-wp");
+		var vnav_alt_wp = getprop("/autopilot/route-manager/route/wp",wp_curr,"altitude-ft");
+		var alt_curr = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+		var dist = getprop("/autopilot/route-manager/wp/dist");
+		var vdist = dist - 1;
+		var alttl = abs(alt_curr - vnav_alt_wp);
+		setprop("/it-autoflight/internal/top-of-des-nm", (alttl / 1000) * 3);
+		if (vdist < getprop("/it-autoflight/internal/top-of-des-nm")) {
+			vnav_des_todt.stop();
+			var salt = getprop("/it-autoflight/internal/alt");
+			var valt = getprop("/it-autoflight/internal/prof-wp-alt");
+			var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+			var sdif = abs(calt - salt);
+			var vdif = abs(calt - valt);
+			if (sdif <= vdif) {
+				setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/alt"));
+			} else if (sdif > vdif) {
+				setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/prof-wp-alt"));
+			}
+			var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+			var valt = getprop("/it-autoflight/internal/prof-alt");
+			var vdif = calt - valt;
+			if (vdif > 550 or vdif < -550) {
+				prof_des_spd();
+			} else {
+				vnav_alt_sel();
+			}
+		}
+	}
+}
+
+# VNAV Altitude Selector
+var vnav_alt_selector = func {
+	var salt = getprop("/it-autoflight/internal/alt");
+	var valt = getprop("/it-autoflight/internal/prof-wp-alt");
+	var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+	var sdif = abs(calt - salt);
+	var vdif = abs(calt - valt);
+	if (getprop("/it-autoflight/internal/prof-mode") == "CLB") {
+		if (sdif <= vdif) {
+			setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/alt"));
+		} else if (sdif > vdif) {
+			setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/prof-wp-alt"));
+		}
+	} else if (getprop("/it-autoflight/internal/prof-mode") == "DES") {
+		var dist = getprop("/autopilot/route-manager/wp/dist");
+		var vdist = dist - 1;
+		if (vdist < getprop("/it-autoflight/internal/top-of-des-nm")) {
+			if (sdif <= vdif) {
+				setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/alt"));
+			} else if (sdif > vdif) {
+				setprop("/it-autoflight/internal/prof-alt", getprop("/it-autoflight/internal/prof-wp-alt"));
+			}
+		}
+	}
+}
+
+# VNAV Selector
+var vnav_alt_sel = func {
+	setprop("/it-autoflight/internal/max-pitch", 8);
+	setprop("/it-autoflight/internal/min-pitch", -5);
+	setprop("/it-autoflight/output/thr-mode", 0);
+	setprop("/it-autoflight/output/prof-vert", 0);
+	setprop("/it-autoflight/mode/thr", "THRUST");
+	setprop("/it-autoflight/mode/prof", "VNAV CAP");
+	vnav_minmaxt.start();
+}
+
+# VNAV Climb
+var prof_clb = func {
+	vnav_des_fpmt.stop();
+	setprop("/it-autoflight/output/thr-mode", 2);
+	setprop("/it-autoflight/mode/thr", " PITCH");
+	setprop("/it-autoflight/output/prof-vert", 4);
+	setprop("/it-autoflight/mode/prof", "VNAV SPD");
+	vnav_altcaptt.start();
+}
+
+# VNAV Descent
+var prof_des_spd = func {
+	vnav_des_fpmt.stop();
+	setprop("/it-autoflight/output/thr-mode", 1);
+	setprop("/it-autoflight/mode/thr", " PITCH");
+	setprop("/it-autoflight/output/prof-vert", 4);
+	setprop("/it-autoflight/mode/prof", "VNAV SPD");
+	vnav_altcaptt.start();
+}
+var prof_des_pth = func {
+	vnav_des_fpmt.start();
+	setprop("/it-autoflight/output/thr-mode", 0);
+	setprop("/it-autoflight/mode/thr", "THRUST");
+	setprop("/it-autoflight/output/prof-vert", 1);
+	setprop("/it-autoflight/mode/prof", "VNAV PTH");
+	vnav_altcaptt.start();
+}
+var vnav_des_fpm = func {
+	if (getprop("/autopilot/route-manager/route/num") > 0 and getprop("/autopilot/route-manager/active") == 1) {
+		var gndspd = getprop("/velocities/groundspeed-kt");
+		var desfpm = ((gndspd * 0.5) * 10);
+		setprop("/it-autoflight/internal/prof-fpm", desfpm);
+	}
+}
+
+# VNAV Capture
+var vnav_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", 150);
+		setprop("/it-autoflight/internal/captvsneg", -150);
+	} else  if ((vsnow >= 1000 and vsnow < 1500) or (vsnow < -1000 and vsnow > -1500)) {
+		setprop("/it-autoflight/internal/captvs", 200);
+		setprop("/it-autoflight/internal/captvsneg", -200);
+	} else  if ((vsnow >= 1500 and vsnow < 2000) or (vsnow < -1500 and vsnow > -2000)) {
+		setprop("/it-autoflight/internal/captvs", 300);
+		setprop("/it-autoflight/internal/captvsneg", -300);
+	} else  if ((vsnow >= 2000 and vsnow < 3000) or (vsnow < -2000 and vsnow > -3000)) {
+		setprop("/it-autoflight/internal/captvs", 450);
+		setprop("/it-autoflight/internal/captvsneg", -450);
+	} else  if ((vsnow >= 3000 and vsnow < 4000) or (vsnow < -3000 and vsnow > -4000)) {
+		setprop("/it-autoflight/internal/captvs", 650);
+		setprop("/it-autoflight/internal/captvsneg", -650);
+	} else  if ((vsnow >= 4000 and vsnow < 5000) or (vsnow < -4000 and vsnow > -5000)) {
+		setprop("/it-autoflight/internal/captvs", 1000);
+		setprop("/it-autoflight/internal/captvsneg", -1000);
+	} else  if ((vsnow >= 5000) or (vsnow < -5000)) {
+		setprop("/it-autoflight/internal/captvs", 1250);
+		setprop("/it-autoflight/internal/captvsneg", -1250);
+	}
+	var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+	var valt = getprop("/it-autoflight/internal/prof-alt");
+	var vdif = calt - valt;
+	if (vdif < getprop("/it-autoflight/internal/captvs") and vdif > getprop("/it-autoflight/internal/captvsneg")) {
+		vnav_capture_alt();
+	}
+}
+
+var vnav_capture_alt = func {
+	vnav_altcaptt.stop();
+	vnav_des_fpmt.stop();
+	var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+	var alt = getprop("/it-autoflight/internal/alt");
+	var valt = getprop("/it-autoflight/internal/prof-alt");
+	var pitchdeg = getprop("/orientation/pitch-deg");
+	if (calt < valt) {
+		setprop("/it-autoflight/internal/max-pitch", pitchdeg);
+	} else if (calt > valt) {
+		setprop("/it-autoflight/internal/min-pitch", pitchdeg);
+	}
+	vnav_minmaxt.start();
+	setprop("/it-autoflight/output/thr-mode", 0);
+	setprop("/it-autoflight/output/prof-vert", 0);
+	setprop("/it-autoflight/mode/thr", "THRUST");
+	setprop("/it-autoflight/mode/prof", "VNAV CAP");
+}
+
+# VNAV Min and Max Pitch Reset
+var vnav_minmax = func {
+	var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
+	var valt = getprop("/it-autoflight/internal/prof-alt");
+	var vdif = calt - valt;
+	if (vdif < 100 and vdif > -100) {
+		setprop("/it-autoflight/internal/max-pitch", 8);
+		setprop("/it-autoflight/internal/min-pitch", -5);
+		var vertmode = getprop("/it-autoflight/output/prof-vert");
+		if (vertmode == 0) {
+			setprop("/it-autoflight/mode/prof", "VNAV HLD");
+		}
+		vnav_minmaxt.stop();
+	}
+}
 
 # CWS
 var cwsroll = func {
@@ -743,6 +1093,10 @@ setlistener("/it-autoflight/input/hdg", func {
 	setprop("/autopilot/settings/heading-bug-deg", getprop("/it-autoflight/input/hdg"));
 });
 
+setlistener("/it-autoflight/internal/alt", func {
+	setprop("/autopilot/settings/target-altitude-ft", getprop("/it-autoflight/internal/alt"));
+});
+
 # Timers
 var altcaptt = maketimer(0.5, altcapt);
 var thrustmodet = maketimer(0.5, thrustmode);
@@ -757,3 +1111,8 @@ var reduct = maketimer(0.5, toga_reduc);
 var latarmt = maketimer(0.5, latarms);
 var fpa_calct = maketimer(0.1, fpa_calc);
 var lnavwptt = maketimer(1, lnavwpt);
+var prof_maint = maketimer(0.5, prof_main);
+var vnav_altcaptt = maketimer(0.5, vnav_altcapt);
+var vnav_minmaxt = maketimer(0.5, vnav_minmax);
+var vnav_des_fpmt = maketimer(0.5, vnav_des_fpm);
+var vnav_des_todt = maketimer(0.5, vnav_des_tod);
diff --git a/Systems/autopilot-dlg.xml b/Systems/autopilot-dlg.xml
index 136e0d5c..2d0f08ac 100644
--- a/Systems/autopilot-dlg.xml
+++ b/Systems/autopilot-dlg.xml
@@ -215,9 +215,8 @@
 			<layout>hbox</layout>
 			
 			<button>
-				<row>0</row>
-				<col>1</col>
 				<legend>SPD</legend>
+				<pref-width>55</pref-width>
 				<binding>
 				<command>property-assign</command>
 				<property>/it-autoflight/input/kts-mach</property>
@@ -226,9 +225,8 @@
 			</button>
 		
 			<button>
-				<row>1</row>
-				<col>1</col>
 				<legend>Mach</legend>
+				<pref-width>55</pref-width>
 				<binding>
 				<command>property-assign</command>
 				<property>/it-autoflight/input/kts-mach</property>
@@ -237,9 +235,21 @@
 			</button>
 			
 			<button>
-				<row>0</row>
-				<col>1</col>
+				<legend>FMS</legend>
+				<pref-width>55</pref-width>
+				<enable>
+					<property>/it-autoflight/input/fms-spd</property>
+				</enable>
+				<binding>
+				<command>property-assign</command>
+				<property>/it-autoflight/input/fms-spd</property>
+				<value>1</value>
+				</binding>
+			</button>
+			
+			<button>
 				<legend>TOGA</legend>
+				<pref-width>55</pref-width>
 				<binding>
 				<command>property-assign</command>
 				<property>/it-autoflight/input/toga</property>
@@ -770,6 +780,38 @@
 			</button>
 		</group>
 		
+		<group>
+			<layout>hbox</layout>
+			
+			<button>
+                <pref-width>55</pref-width>
+				<legend>VNAV</legend>
+				<enable>
+					<property>/it-autoflight/settings/vnav-test</property>
+				</enable>
+				<binding>
+				<command>property-assign</command>
+				<property>/it-autoflight/input/vert</property>
+				<value>8</value>
+				</binding>
+			</button>
+			
+			<text>
+				<row>0</row>
+				<col>0</col>
+				<label>VNAV Command: </label>
+			</text>
+			
+			<text>
+				<border>1</border>
+				<label>9999999999999</label>
+				<valign>center</valign>
+				<property>/it-autoflight/mode/prof</property>
+				<live>1</live>
+			</text>
+			
+		</group>
+		
 		<group>
 			<layout>table</layout>
 			
@@ -956,8 +998,8 @@
 				<binding>
 					<command>property-adjust</command>
 					<property>/it-autoflight/input/fpa</property>
-					<min>-9.9</min>
-					<max>9.9</max>
+					<min>-20.9</min>
+					<max>20.9</max>
 					<step>-1</step>
 					<wrap>false</wrap>
 				</binding>
@@ -971,8 +1013,8 @@
 				<binding>
 					<command>property-adjust</command>
 					<property>/it-autoflight/input/fpa</property>
-					<min>-9.9</min>
-					<max>9.9</max>
+					<min>-20.9</min>
+					<max>20.9</max>
 					<step>-0.1</step>
 					<wrap>false</wrap>
 				</binding>
@@ -998,8 +1040,8 @@
 				<binding>
 					<command>property-adjust</command>
 					<property>/it-autoflight/input/fpa</property>
-					<min>-9.9</min>
-					<max>9.9</max>
+					<min>-20.9</min>
+					<max>20.9</max>
 					<step>0.1</step>
 					<wrap>false</wrap>
 				</binding>
@@ -1014,8 +1056,8 @@
 				<binding>
 					<command>property-adjust</command>
 					<property>/it-autoflight/input/fpa</property>
-					<min>-9.9</min>
-					<max>9.9</max>
+					<min>-20.9</min>
+					<max>20.9</max>
 					<step>1</step>
 					<wrap>false</wrap>
 				</binding>
diff --git a/Systems/custom-autothrust.xml b/Systems/custom-autothrust.xml
index 197035fb..398f964a 100644
--- a/Systems/custom-autothrust.xml
+++ b/Systems/custom-autothrust.xml
@@ -42,11 +42,10 @@
       <property>/controls/engines/engine[0]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
@@ -91,11 +90,10 @@
       <property>/controls/engines/engine[0]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
@@ -133,11 +131,10 @@
       <property>/controls/engines/engine[0]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.08</u_min>  
@@ -175,11 +172,10 @@
       <property>/controls/engines/engine[0]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
@@ -222,11 +218,10 @@
       <property>/controls/engines/engine[1]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
@@ -271,11 +266,10 @@
       <property>/controls/engines/engine[1]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
@@ -313,11 +307,10 @@
       <property>/controls/engines/engine[1]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.08</u_min>  
@@ -355,11 +348,10 @@
       <property>/controls/engines/engine[1]/throttle-cmd</property>
     </output>
     <config>
-      <Kp>0.1</Kp>        
+      <Kp>0.01</Kp>        
       <beta>1.0</beta>    
       <alpha>0.1</alpha>  
       <gamma>0.0</gamma>  
-                          
       <Ti>10.0</Ti>       
       <Td>0.00001</Td>    
       <u_min>0.0</u_min>  
diff --git a/Systems/it-autoflight.xml b/Systems/it-autoflight.xml
index 81f564c2..f98f02e4 100644
--- a/Systems/it-autoflight.xml
+++ b/Systems/it-autoflight.xml
@@ -39,228 +39,220 @@
  <!-- Autopilot Helpers                                               -->
  <!-- =============================================================== -->
   
-  <filter>
-    <name>MAGNETIC HDG ERROR DEG</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-      <property>/it-autoflight/input/hdg</property>
-      <offset>
-        <property>/orientation/heading-magnetic-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/heading-bug-error-deg</output>
-    <output>/it-autoflight/internal/fdm-heading-bug-error-deg</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
+	<filter>
+		<name>MAGNETIC HDG ERROR DEG</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<property>/it-autoflight/input/hdg</property>
+			<offset>
+				<property>/orientation/heading-magnetic-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/heading-bug-error-deg</output>
+		<output>/it-autoflight/internal/fdm-heading-bug-error-deg</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
-  <filter>
-    <name>TRUE HDG ERROR DEG</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-      <property>/it-autoflight/input/hdg</property>
-      <offset>
-        <property>/orientation/heading-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/true-heading-error-deg</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
+	<filter>
+		<name>TRUE HDG ERROR DEG</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<property>/it-autoflight/input/hdg</property>
+			<offset>
+				<property>/orientation/heading-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/true-heading-error-deg</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
-  <filter>
-    <name>LNAV HDG ERROR DEG</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-      <property>/autopilot/settings/true-heading-deg</property>
-      <offset>
-        <property>/orientation/heading-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/lnav-hdg-error-deg</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
+	<filter>
+		<name>LNAV HDG ERROR DEG</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<property>/autopilot/settings/true-heading-deg</property>
+			<offset>
+				<property>/orientation/heading-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/lnav-hdg-error-deg</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
-  <filter>
-    <name>NAV ERROR DEG</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-	  <condition>
-		<equals>
-		  <property>/it-autoflight/settings/use-nav2-radio</property>
-		  <value>0</value>
-		</equals>
-	  </condition>
-      <property>/instrumentation/nav[0]/radials/target-auto-hdg-deg</property>
-      <offset>
-        <property>/orientation/heading-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <input>
-	  <condition>
-		<equals>
-		  <property>/it-autoflight/settings/use-nav2-radio</property>
-		  <value>1</value>
-		</equals>
-	  </condition>
-      <property>/instrumentation/nav[1]/radials/target-auto-hdg-deg</property>
-      <offset>
-        <property>/orientation/heading-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/nav-heading-error-deg</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
+	<filter>
+		<name>NAV ERROR DEG</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/settings/use-nav2-radio</property>
+					<value>0</value>
+				</equals>
+			</condition>
+			<property>/instrumentation/nav[0]/radials/target-auto-hdg-deg</property>
+			<offset>
+				<property>/orientation/heading-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/settings/use-nav2-radio</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<property>/instrumentation/nav[1]/radials/target-auto-hdg-deg</property>
+			<offset>
+				<property>/orientation/heading-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/nav-heading-error-deg</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
-  <filter>
-    <name>NAV COURSE ERROR</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-	  <condition>
-		<equals>
-		  <property>/it-autoflight/settings/use-nav2-radio</property>
-		  <value>0</value>
-		</equals>
-	  </condition>
-      <property>/instrumentation/nav[0]/radials/selected-deg</property>
-      <offset>
-        <property>/orientation/heading-magnetic-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <input>
-	  <condition>
-		<equals>
-		  <property>/it-autoflight/settings/use-nav2-radio</property>
-		  <value>1</value>
-		</equals>
-	  </condition>
-      <property>/instrumentation/nav[1]/radials/selected-deg</property>
-      <offset>
-        <property>/orientation/heading-magnetic-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/nav-course-error</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
-  
-  <!--filter>
-    <name>NAV BC ERROR DEG</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-	  <expression>
-	    <sum>
-	      <property>/it-autoflight/internal/nav-heading-error-deg</property>
-		  <value>180</value>
-		</sum>
-	  </expression>
-	</input>
-    <output>/it-autoflight/internal/nav-bc-heading-error-deg</output>
-    <gain>1.0</gain>
-  </filter-->
+	<filter>
+		<name>NAV COURSE ERROR</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/settings/use-nav2-radio</property>
+					<value>0</value>
+				</equals>
+			</condition>
+			<property>/instrumentation/nav[0]/radials/selected-deg</property>
+			<offset>
+				<property>/orientation/heading-magnetic-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/settings/use-nav2-radio</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<property>/instrumentation/nav[1]/radials/selected-deg</property>
+			<offset>
+				<property>/orientation/heading-magnetic-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/nav-course-error</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
-  <filter>
-    <name>INTERNAL VERTICAL SPEED COMPUTER</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>/velocities/vertical-speed-fps</input>
-    <output>/it-autoflight/internal/vert-speed-fpm</output>
-    <gain>60.0</gain>
-  </filter>
+	<filter>
+		<name>INTERNAL VERTICAL SPEED COMPUTER</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>/velocities/vertical-speed-fps</input>
+		<output>/it-autoflight/internal/vert-speed-fpm</output>
+		<gain>60.0</gain>
+	</filter>
 
-  <predict-simple>
-    <name>IAS 5 SECOND PREDICTOR</name>
-    <debug>false</debug>
-    <input>/velocities/airspeed-kt</input>
-    <output>/it-autoflight/internal/lookahead-5-sec-airspeed-kt</output>
-    <seconds>5.0</seconds>
-    <filter-gain>0.0</filter-gain>
-  </predict-simple>
+	<predict-simple>
+		<name>IAS 5 SECOND PREDICTOR</name>
+		<debug>false</debug>
+		<input>/instrumentation/airspeed-indicator/indicated-speed-kt</input>
+		<output>/it-autoflight/internal/lookahead-5-sec-airspeed-kt</output>
+		<seconds>5.0</seconds>
+		<filter-gain>0.0</filter-gain>
+	</predict-simple>
 
-  <predict-simple>
-    <name>IAS 10 SECOND PREDICTOR</name>
-    <debug>false</debug>
-    <input>/velocities/airspeed-kt</input>
-    <output>/it-autoflight/internal/lookahead-10-sec-airspeed-kt</output>
-    <seconds>10.0</seconds>
-    <filter-gain>0.0</filter-gain>
-  </predict-simple>
-  
-  <predict-simple>
-    <name>MACH 10 SECOND PREDICTOR</name>
-    <debug>false</debug>
-    <input>/velocities/mach</input>
-    <output>/it-autoflight/internal/lookahead-10-sec-mach</output>
-    <seconds>10.0</seconds>
-    <filter-gain>0.0</filter-gain>
-  </predict-simple>
+	<predict-simple>
+		<name>IAS 10 SECOND PREDICTOR</name>
+		<debug>false</debug>
+		<input>/instrumentation/airspeed-indicator/indicated-speed-kt</input>
+		<output>/it-autoflight/internal/lookahead-10-sec-airspeed-kt</output>
+		<seconds>10.0</seconds>
+		<filter-gain>0.0</filter-gain>
+	</predict-simple>
+	
+	<predict-simple>
+		<name>IAS 15 SECOND PREDICTOR</name>
+		<debug>false</debug>
+		<input>/instrumentation/airspeed-indicator/indicated-speed-kt</input>
+		<output>/it-autoflight/internal/lookahead-15-sec-airspeed-kt</output>
+		<seconds>15.0</seconds>
+		<filter-gain>0.0</filter-gain>
+	</predict-simple>
 
-  <filter>
-    <name>STATIC PRESSURE COMPUTER</name>
-    <debug>false</debug>
-    <type>derivative</type>
-    <input>/systems/static[0]/pressure-inhg</input>
-    <output>/it-autoflight/internal/pressure-rate</output>
-    <filter-time>1.0</filter-time>
-  </filter>
+	<predict-simple>
+		<name>MACH 10 SECOND PREDICTOR</name>
+		<debug>false</debug>
+		<input>/instrumentation/airspeed-indicator/indicated-mach</input>
+		<output>/it-autoflight/internal/lookahead-10-sec-mach</output>
+		<seconds>10.0</seconds>
+		<filter-gain>0.0</filter-gain>
+	</predict-simple>
 
-  <filter>
-    <name>NAV1 TRACK ERROR</name>
-    <debug>false</debug>
-    <type>gain</type>
-    <input>
-      <property>/instrumentation/nav[0]/radials/target-auto-hdg-deg</property>
-      <offset>
-        <property>/orientation/track-deg</property>
-        <scale>-1.0</scale>
-      </offset>
-    </input>
-    <output>/it-autoflight/internal/nav1-track-error-deg</output>
-    <period>
-      <min>-180</min>
-      <max>180</max>
-    </period>
-    <gain>1.0</gain>
-  </filter>
+	<filter>
+		<name>STATIC PRESSURE COMPUTER</name>
+		<debug>false</debug>
+		<type>derivative</type>
+		<input>/systems/static[0]/pressure-inhg</input>
+		<output>/it-autoflight/internal/pressure-rate</output>
+		<filter-time>1.0</filter-time>
+	</filter>
+
+	<filter>
+		<name>NAV1 TRACK ERROR</name>
+		<debug>false</debug>
+		<type>gain</type>
+		<input>
+			<property>/instrumentation/nav[0]/radials/target-auto-hdg-deg</property>
+			<offset>
+				<property>/orientation/track-deg</property>
+				<scale>-1.0</scale>
+			</offset>
+		</input>
+		<output>/it-autoflight/internal/nav1-track-error-deg</output>
+		<period>
+			<min>-180</min>
+			<max>180</max>
+		</period>
+		<gain>1.0</gain>
+	</filter>
 
  <!-- =============================================================== -->
  <!-- Lateral  Modes                                                  -->
  <!-- =============================================================== -->
  
-	<filter>
-		<name>TARGET ROLL CONTROLLER</name>
+	<pi-simple-controller>
+		<name>ROLL CONTROLLER: HDG/LNAV</name>
 		<debug>false</debug>
-		<type>gain</type>
 		<enable>
 			<condition>
 				<or>
@@ -272,14 +264,6 @@
 						<property>/it-autoflight/output/lat</property>
 						<value>1</value>
 					</equals>
-					<equals>
-						<property>/it-autoflight/output/lat</property>
-						<value>4</value>
-					</equals>
-					<equals>
-						<property>/it-autoflight/output/lat</property>
-						<value>5</value>
-					</equals>
 				</or>
 			</condition>
 		</enable>
@@ -331,63 +315,56 @@
 				<property>/it-autoflight/config/cmd/roll-scale</property>
 			</scale>
 		</input>
-		<input>
-			<condition>
-				<or>
-					<equals>
-						<property>/it-autoflight/output/lat</property>
-						<value>4</value>
-					</equals>
-					<equals>
-						<property>/it-autoflight/output/lat</property>
-						<value>5</value>
-					</equals>
-				</or>
-			</condition>
+		<reference>
 			<value>0</value>
-		</input>
+		</reference>
 		<output>
 			<property>/it-autoflight/internal/target-roll-deg</property>
 		</output>
-		<gain>0.5</gain>
-		<u_min>
-			<condition>
-				<equals>
-					<property>/it-autoflight/output/lat</property>
-					<value>0</value>
-				</equals>
-			</condition>
-			<property>/it-autoflight/input/bank-limit</property>
-			<scale>-1</scale>
-		</u_min>
-		<u_min>
-			<condition>
-				<equals>
-					<property>/it-autoflight/output/lat</property>
-					<value>1</value>
-				</equals>
-			</condition>
-			<value>-20</value>
-		</u_min>
-		<u_max>
-			<condition>
-				<equals>
-					<property>/it-autoflight/output/lat</property>
-					<value>0</value>
-				</equals>
-			</condition>
-			<property>/it-autoflight/input/bank-limit</property>
-		</u_max>
-		<u_max>
-			<condition>
-				<equals>
-					<property>/it-autoflight/output/lat</property>
-					<value>1</value>
-				</equals>
-			</condition>
-			<value>20</value>
-		</u_max>
-	</filter>
+		<config>
+			<Kp>
+				<property>/it-autoflight/config/cmd/roll-kp</property>
+			</Kp>
+			<Ki>0.0</Ki>
+			<min>
+				<condition>
+					<not-equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>1</value>
+					</not-equals>
+				</condition>
+				<property>/it-autoflight/input/bank-limit</property>
+				<scale>-1</scale>
+			</min>
+			<min>
+				<condition>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>1</value>
+					</equals>
+				</condition>
+				<value>-25</value>
+			</min>
+			<max>
+				<condition>
+					<not-equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>1</value>
+					</not-equals>
+				</condition>
+				<property>/it-autoflight/input/bank-limit</property>
+			</max>
+			<max>
+				<condition>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>1</value>
+					</equals>
+				</condition>
+				<value>25</value>
+			</max>
+		</config>
+	</pi-simple-controller>
 	
 	<pid-controller>
 		<name>VORLOC TRK</name>
@@ -446,9 +423,9 @@
 		<input>
 			<property>/it-autoflight/internal/target-roll-deg</property>
 		</input>
-		<output>/it-autoflight/internal/roll-deg-cmd</output>
+		<output>/it-autoflight/internal/target-roll</output>
 		<type>noise-spike</type>
-		<max-rate-of-change>10</max-rate-of-change>
+		<max-rate-of-change>15</max-rate-of-change>
 	</filter>
 	
 	<pid-controller>
@@ -472,7 +449,38 @@
 			<property>/orientation/roll-deg</property>
 		</input>
 		<reference>
-			<property>/it-autoflight/internal/roll-deg-cmd</property>
+			<condition>
+				<or>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>0</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>2</value>
+					</equals>
+				</or>
+			</condition>
+			<property>/it-autoflight/internal/target-roll</property>
+		</reference>
+		<reference>
+			<condition>
+				<or>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>4</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/lat</property>
+						<value>5</value>
+					</equals>
+				</or>
+			</condition>
+			<value>0</value>
 		</reference>
 		<output>
 			<property>/it-autoflight/internal/aileron-cmd</property>
@@ -547,7 +555,7 @@
 		</input>
 		<output>/controls/flight/aileron</output>
 		<type>noise-spike</type>
-		<max-rate-of-change>0.8</max-rate-of-change>
+		<max-rate-of-change>0.9</max-rate-of-change>
 	</filter>
 
  <!-- =============================================================== -->
@@ -559,18 +567,45 @@
 		<debug>false</debug>
 		<enable>
 			<condition>
-				<equals>
-					<property>/it-autoflight/output/vert</property>
-					<value>0</value>
-				</equals>
+				<or>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>0</value>
+					</equals>
+					<and>
+						<equals>
+							<property>/it-autoflight/output/vert</property>
+							<value>8</value>
+						</equals>
+						<equals>
+							<property>/it-autoflight/output/prof-vert</property>
+							<value>0</value>
+						</equals>
+					</and>
+				</or>
 			</condition>
 		</enable>
 		<input>
 			<property>/it-autoflight/internal/altitude-5-sec-ahead</property>
 		</input>
 		<reference>
+			<condition>
+				<equals>
+					<property>/it-autoflight/output/vert</property>
+					<value>0</value>
+				</equals>
+			</condition>
 			<property>/it-autoflight/internal/alt</property>
 		</reference>
+		<reference>
+			<condition>
+				<equals>
+					<property>/it-autoflight/output/vert</property>
+					<value>8</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/internal/prof-alt</property>
+		</reference>
 		<output>
 			<property>/it-autoflight/internal/target-pitch-deg</property>
 		</output>
@@ -592,45 +627,6 @@
 		</config>
 	</pid-controller>
 	
-	<pid-controller>
-		<name>VERTICAL SPEED HOLD</name>
-		<debug>false</debug>
-		<enable>
-			<condition>
-				<equals>
-					<property>/it-autoflight/output/vert</property>
-					<value>1</value>
-				</equals>
-			</condition>
-		</enable>
-		<input>
-			<property>/velocities/vertical-speed-fps</property>
-		</input>
-		<reference>
-			<property>/it-autoflight/input/vs</property>
-			<scale>0.01666666</scale>
-		</reference>
-		<output>
-			<property>/it-autoflight/internal/target-pitch-deg</property>
-		</output>
-		<config>
-			<Kp>
-				<property>/it-autoflight/config/cmd/vs</property>
-			</Kp>
-			<beta>1.0</beta>
-			<alpha>0.1</alpha>
-			<gamma>0.0</gamma>
-			<Ti>5.0</Ti>
-			<Td>0.00001</Td>
-			<u_min>
-				<value>-10</value>
-			</u_min>
-			<u_max>
-				<value>30</value>
-			</u_max>
-		</config>
-	</pid-controller>
-	
 	<pid-controller>
 		<name>FPA HOLD</name>
 		<debug>false</debug>
@@ -760,16 +756,20 @@
 	</pid-controller>
 	
 	<pid-controller>
-		<name>FLIGHT LEVEL CHANGE</name>
+		<name>FLCH SPEED BY PITCH</name>
 		<debug>false</debug>
 		<enable>
 			<condition>
-				<or>
-					<equals>
+				<and>
+					<not-equals>
 						<property>/it-autoflight/output/vert</property>
-						<value>4</value>
-					</equals>
-				</or>
+						<value>7</value>
+					</not-equals>
+					<not-equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>8</value>
+					</not-equals>
+				</and>
 			</condition>
 		</enable>
 		<input>
@@ -779,7 +779,7 @@
 					<value>0</value>
 				</equals>
 			</condition>
-			<property>/it-autoflight/internal/lookahead-10-sec-airspeed-kt</property>
+			<property>/it-autoflight/internal/lookahead-15-sec-airspeed-kt</property>
 		</input>
 		<input>
 			<condition>
@@ -789,7 +789,7 @@
 				</equals>
 			</condition>
 			<property>/it-autoflight/internal/lookahead-10-sec-mach</property>
-			<scale>1000.0</scale>
+			<scale>500.0</scale>
 		</input>
 		<reference>
 			<condition>
@@ -808,10 +808,10 @@
 				</equals>
 			</condition>
 			<property>/it-autoflight/input/spd-mach</property>
-			<scale>1000.0</scale>
+			<scale>500.0</scale>
 		</reference>
 		<output>
-			<property>/it-autoflight/internal/target-pitch-deg</property>
+			<property>/it-autoflight/internal/target-fps</property>
 		</output>
 		<config>
 			<Kp>
@@ -829,7 +829,7 @@
                         <property>/instrumentation/altimeter/indicated-altitude-ft</property>
                     </greater-than>
                 </condition>
-                <value>2</value>
+                <value>1.6</value>
             </u_min>
             <u_min>
                 <condition>
@@ -838,7 +838,7 @@
                         <property>/instrumentation/altimeter/indicated-altitude-ft</property>
                     </less-than>
                 </condition>
-                <value>-7.0</value>
+                <value>-100</value>
             </u_min>
             <u_max>
                 <condition>
@@ -847,7 +847,7 @@
                         <property>/instrumentation/altimeter/indicated-altitude-ft</property>
                     </greater-than>
                 </condition>
-                <value>25.0</value>
+                <value>166</value>
             </u_max>
             <u_max>
                 <condition>
@@ -856,7 +856,7 @@
                         <property>/instrumentation/altimeter/indicated-altitude-ft</property>
                     </less-than>
                 </condition>
-                <value>0.0</value>
+                <value>-1.6</value>
             </u_max>
 		</config>
 	</pid-controller>
@@ -866,16 +866,14 @@
 		<debug>false</debug>
 		<enable>
 			<condition>
-				<or>
-					<equals>
-						<property>/it-autoflight/output/vert</property>
-						<value>7</value>
-					</equals>
-				</or>
+				<equals>
+					<property>/it-autoflight/output/vert</property>
+					<value>7</value>
+				</equals>
 			</condition>
 		</enable>
 		<input>
-			<property>/it-autoflight/internal/lookahead-10-sec-airspeed-kt</property>
+			<property>/it-autoflight/internal/lookahead-15-sec-airspeed-kt</property>
 		</input>
 		<reference>
 			<condition>
@@ -896,7 +894,7 @@
 			<property>/it-autoflight/input/spd-kts</property>
 		</reference>
 		<output>
-			<property>/it-autoflight/internal/target-pitch-deg</property>
+			<property>/it-autoflight/internal/target-fps</property>
 		</output>
 		<config>
 			<Kp>
@@ -908,14 +906,227 @@
 			<Ti>10.0</Ti>        
 			<Td>0.00001</Td>    
             <u_min>
-                <value>2</value>
+                <value>1.6</value>
             </u_min>
             <u_max>
-                <value>25.0</value>
+                <value>166</value>
             </u_max>
 		</config>
 	</pid-controller>
 	
+	<pid-controller>
+		<name>VNAV SPEED BY PITCH</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<equals>
+					<property>/it-autoflight/output/vert</property>
+					<value>8</value>
+				</equals>
+			</condition>
+		</enable>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/kts-mach</property>
+					<value>0</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/internal/lookahead-15-sec-airspeed-kt</property>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/kts-mach</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/internal/lookahead-10-sec-mach</property>
+			<scale>500.0</scale>
+		</input>
+		<reference>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/kts-mach</property>
+					<value>0</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/input/spd-kts</property>
+		</reference>
+		<reference>
+			<condition>
+				<equals>
+					<property>/it-autoflight/input/kts-mach</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/input/spd-mach</property>
+			<scale>500.0</scale>
+		</reference>
+		<output>
+			<property>/it-autoflight/internal/target-fps</property>
+		</output>
+		<config>
+			<Kp>
+				<property>/it-autoflight/config/cmd/flch</property>
+			</Kp>
+			<beta>1.0</beta>    
+			<alpha>0.1</alpha>  
+			<gamma>0.0</gamma>  
+			<Ti>10.0</Ti>        
+			<Td>0.00001</Td>    
+            <u_min>
+                <condition>
+                    <greater-than>
+                        <property>/it-autoflight/internal/prof-alt</property>
+                        <property>/instrumentation/altimeter/indicated-altitude-ft</property>
+                    </greater-than>
+                </condition>
+                <value>1.6</value>
+            </u_min>
+            <u_min>
+                <condition>
+                    <less-than>
+                        <property>/it-autoflight/internal/prof-alt</property>
+                        <property>/instrumentation/altimeter/indicated-altitude-ft</property>
+                    </less-than>
+                </condition>
+                <value>-100</value>
+            </u_min>
+            <u_max>
+                <condition>
+                    <greater-than>
+                        <property>/it-autoflight/internal/prof-alt</property>
+                        <property>/instrumentation/altimeter/indicated-altitude-ft</property>
+                    </greater-than>
+                </condition>
+                <value>166</value>
+            </u_max>
+            <u_max>
+                <condition>
+                    <less-than>
+                        <property>/it-autoflight/internal/prof-alt</property>
+                        <property>/instrumentation/altimeter/indicated-altitude-ft</property>
+                    </less-than>
+                </condition>
+                <value>-1.6</value>
+            </u_max>
+		</config>
+	</pid-controller>
+	
+	<pid-controller>
+		<name>FPM HOLD</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<or>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>4</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>7</value>
+					</equals>
+					<and>
+						<equals>
+							<property>/it-autoflight/output/vert</property>
+							<value>8</value>
+						</equals>
+						<or>
+							<equals>
+								<property>/it-autoflight/output/prof-vert</property>
+								<value>1</value>
+							</equals>
+							<equals>
+								<property>/it-autoflight/output/prof-vert</property>
+								<value>4</value>
+							</equals>
+						</or>
+					</and>
+				</or>
+			</condition>
+		</enable>
+		<input>
+			<property>/instrumentation/vertical-speed-indicator/indicated-speed-fpm</property>
+			<scale>0.01666666</scale>
+		</input>
+		<reference>
+			<condition>
+				<equals>
+					<property>/it-autoflight/output/vert</property>
+					<value>1</value>
+				</equals>
+			</condition>
+			<property>/it-autoflight/input/vs</property>
+			<scale>0.01666666</scale>
+		</reference>
+		<reference>
+			<condition>
+				<or>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>4</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>7</value>
+					</equals>
+					<and>
+						<equals>
+							<property>/it-autoflight/output/vert</property>
+							<value>8</value>
+						</equals>
+						<equals>
+							<property>/it-autoflight/output/prof-vert</property>
+							<value>4</value>
+						</equals>
+					</and>
+				</or>
+			</condition>
+			<property>/it-autoflight/internal/target-fps</property>
+		</reference>
+		<reference>
+			<condition>
+				<and>
+					<equals>
+						<property>/it-autoflight/output/vert</property>
+						<value>8</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/prof-vert</property>
+						<value>1</value>
+					</equals>
+				</and>
+			</condition>
+			<property>/it-autoflight/internal/prof-fpm</property>
+			<scale>-0.01666666</scale>
+		</reference>
+		<output>
+			<property>/it-autoflight/internal/target-pitch-deg</property>
+		</output>
+		<config>
+			<Kp>
+				<property>/it-autoflight/config/cmd/vs</property>
+			</Kp>
+			<beta>1.0</beta>
+			<alpha>0.1</alpha>
+			<gamma>0.0</gamma>
+			<Ti>5.0</Ti>
+			<Td>0.00001</Td>
+			<u_min>
+				<value>-10</value>
+			</u_min>
+			<u_max>
+				<value>25</value>
+			</u_max>
+		</config>
+	</pid-controller>
+	
 	<filter>
 		<name>IT-CONTROLLER: TARGET PITCH CMD</name>
 		<debug>false</debug>
@@ -924,7 +1135,7 @@
 		<input>
 			<property>/it-autoflight/internal/target-pitch-deg</property>
 		</input>
-		<output>/it-autoflight/internal/pitch-deg-cmd</output>
+		<output>/it-autoflight/internal/target-pitch</output>
 		<type>noise-spike</type>
 		<max-rate-of-change>10</max-rate-of-change>
 	</filter>
@@ -950,7 +1161,7 @@
 			<property>/orientation/pitch-deg</property>
 		</input>
 		<reference>
-			<property>/it-autoflight/internal/pitch-deg-cmd</property>
+			<property>/it-autoflight/internal/target-pitch</property>
 		</reference>
 		<output>
 			<property>/it-autoflight/internal/elevator-cmd</property>
@@ -1001,7 +1212,7 @@
 		</input>
 		<output>/controls/flight/elevator</output>
 		<type>noise-spike</type>
-		<max-rate-of-change>0.3</max-rate-of-change>
+		<max-rate-of-change>0.4</max-rate-of-change>
 	</filter>
 	
  <!-- =============================================================== -->
@@ -1370,7 +1581,7 @@
 		<input>
 			<expression>
 				<dif>
-					<property>/it-autoflight/internal/roll-deg-cmd</property>
+					<property>/it-autoflight/internal/target-roll</property>
 					<property>/orientation/roll-deg</property>
 				</dif>
 			</expression>
@@ -1387,7 +1598,7 @@
 		<input>
 			<expression>
 				<dif>
-					<property>/it-autoflight/internal/pitch-deg-cmd</property>
+					<property>/it-autoflight/internal/target-pitch</property>
 					<property>/orientation/pitch-deg</property>
 				</dif>
 			</expression>
diff --git a/Systems/it-autothrust.xml b/Systems/it-autothrust.xml
index 3080687b..eaaf3ff5 100644
--- a/Systems/it-autothrust.xml
+++ b/Systems/it-autothrust.xml
@@ -8,227 +8,207 @@
  <!-- Thrust Modes                                                    -->
  <!-- =============================================================== -->
 
- <!-- Auto throttle --> 
- <pid-controller>
-    <name>IAS THR</name>
-    <debug>false</debug>
-    <enable>
-	  <condition>
-		<and>
-		  <equals>
-			<property>/it-autoflight/input/kts-mach</property>
-			<value>0</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/thr-mode</property>
-			<value>0</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/athr</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/retard</property>
-			<value>0</value>
-		  </equals>
-		</and>
-	  </condition>
-    </enable>
-    <input>
-      <property>/velocities/airspeed-kt</property>
-    </input>
-    <reference>
-      <property>/it-autoflight/input/spd-kts</property>
-    </reference>
-    <output>
-      <property>/controls/flight/throttle-cmd</property>
-    </output>
-    <config>
-      <Kp>0.1</Kp>        
-      <beta>1.0</beta>    
-      <alpha>0.1</alpha>  
-      <gamma>0.0</gamma>  
-      <Ti>10.0</Ti>       
-      <Td>0.00001</Td>    
-      <u_min>0.0</u_min>  
-      <u_max>0.94</u_max>  
-    </config>
-  </pid-controller>
+	<!-- Auto throttle --> 
+	<pid-controller>
+		<name>IAS THR</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<and>
+					<equals>
+						<property>/it-autoflight/input/kts-mach</property>
+						<value>0</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/thr-mode</property>
+						<value>0</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/athr</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/retard</property>
+						<value>0</value>
+					</equals>
+				</and>
+			</condition>
+		</enable>
+		<input>
+			<property>/instrumentation/airspeed-indicator/indicated-speed-kt</property>
+		</input>
+		<reference>
+			<property>/it-autoflight/input/spd-kts</property>
+		</reference>
+		<output>
+			<property>/controls/flight/throttle-cmd</property>
+		</output>
+		<config>
+			<Kp>0.1</Kp>        
+			<beta>1.0</beta>    
+			<alpha>0.1</alpha>  
+			<gamma>0.0</gamma>  
+			<Ti>10.0</Ti>       
+			<Td>0.00001</Td>    
+			<u_min>0.0</u_min>  
+			<u_max>0.94</u_max>  
+		</config>
+	</pid-controller>
 
-<!-- Auto throttle (Mach Hold)-->
+	<!-- Auto throttle (Mach Hold)-->
 
- <pid-controller>
-    <name>MACH THR</name>
-    <debug>false</debug>
-    <enable>
-	  <condition>
-		<and>
-		  <equals>
-			<property>/it-autoflight/input/kts-mach</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/thr-mode</property>
+	<pid-controller>
+		<name>MACH THR</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<and>
+					<equals>
+						<property>/it-autoflight/input/kts-mach</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/thr-mode</property>
+						<value>0</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/athr</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/retard</property>
+						<value>0</value>
+					</equals>
+				</and>
+			</condition>
+		</enable>
+		<input>
+			<property>/instrumentation/airspeed-indicator/indicated-mach</property>
+			<scale>1000.0</scale>
+		</input>
+		<reference>
+			<property>/it-autoflight/input/spd-mach</property>
+			<scale>1000.0</scale>
+		</reference>
+		<output>
+			<property>/controls/flight/throttle-cmd</property>
+		</output>
+		<config>
+			<Kp>0.1</Kp>        
+			<beta>1.0</beta>    
+			<alpha>0.1</alpha>  
+			<gamma>0.0</gamma>  
+			<Ti>10.0</Ti>       
+			<Td>0.00001</Td>    
+			<u_min>0.0</u_min>  
+			<u_max>0.94</u_max>  
+		</config>
+	</pid-controller>
+
+	<!-- IDLE THR -->
+	<pid-controller>
+		<name>IDLE</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<and>
+					<or>
+						<equals>
+							<property>/it-autoflight/output/thr-mode</property>
+							<value>1</value>
+						</equals>
+						<equals>
+							<property>/it-autoflight/output/retard</property>
+							<value>1</value>
+						</equals>
+					</or>
+					<equals>
+						<property>/it-autoflight/output/athr</property>
+						<value>1</value>
+					</equals>
+				</and>
+			</condition>
+		</enable>
+		<input>
+		<value>900</value>
+		</input>
+		<reference>
 			<value>0</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/athr</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/retard</property>
+		</reference>
+		<output>
+			<property>/controls/flight/throttle-cmd</property>
+		</output>
+		<config>
+			<Kp>0.1</Kp>        
+			<beta>1.0</beta>    
+			<alpha>0.1</alpha>  
+			<gamma>0.0</gamma>  
+			<Ti>10.0</Ti>       
+			<Td>0.00001</Td>    
+			<u_min>
+				<condition>
+					<equals>
+						<property>/it-autoflight/output/retard</property>
+						<value>0</value>
+					</equals>
+				</condition>
+				<value>0.08</value>
+			</u_min>  
+			<u_min>
+				<condition>
+					<equals>
+						<property>/it-autoflight/output/retard</property>
+						<value>1</value>
+					</equals>
+				</condition>
+				<value>0</value>
+			</u_min>
+			<u_max>0.2</u_max>  
+		</config>
+	</pid-controller>
+
+	<!-- CLB THR -->
+	<pid-controller>
+		<name>CLB</name>
+		<debug>false</debug>
+		<enable>
+			<condition>
+				<and>
+					<equals>
+						<property>/it-autoflight/output/thr-mode</property>
+						<value>2</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/athr</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/it-autoflight/output/retard</property>
+						<value>0</value>
+					</equals>
+				</and>
+			</condition>
+		</enable>
+		<input>
 			<value>0</value>
-		  </equals>
-		</and>
-	  </condition>
-    </enable>
-    <input>
-      <property>/velocities/mach</property>
-	  <scale>1000.0</scale>
-    </input>
-    <reference>
-      <property>/it-autoflight/input/spd-mach</property>
-	  <scale>1000.0</scale>
-    </reference>
-    <output>
-      <property>/controls/flight/throttle-cmd</property>
-    </output>
-    <config>
-      <Kp>0.1</Kp>        
-      <beta>1.0</beta>    
-      <alpha>0.1</alpha>  
-      <gamma>0.0</gamma>  
-      <Ti>10.0</Ti>       
-      <Td>0.00001</Td>    
-      <u_min>0.0</u_min>  
-      <u_max>0.94</u_max>  
-    </config>
-  </pid-controller>
-  
-<!-- IDLE THR -->
- <pid-controller>
-    <name>IDLE</name>
-    <debug>false</debug>
-    <enable>
-	  <condition>
-		<and>
-		  <equals>
-			<property>/it-autoflight/output/thr-mode</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/athr</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/retard</property>
-			<value>0</value>
-		  </equals>
-		</and>
-	  </condition>
-    </enable>
-    <input>
-      <property>/velocities/airspeed-kt</property>
-    </input>
-    <reference>
-      <value>0</value>
-    </reference>
-    <output>
-      <property>/controls/flight/throttle-cmd</property>
-    </output>
-    <config>
-      <Kp>0.1</Kp>        
-      <beta>1.0</beta>    
-      <alpha>0.1</alpha>  
-      <gamma>0.0</gamma>  
-      <Ti>10.0</Ti>       
-      <Td>0.00001</Td>    
-      <u_min>0.08</u_min>  
-      <u_max>0.2</u_max>  
-    </config>
-  </pid-controller>
-  
-  <!-- CLB THR -->
- <pid-controller>
-    <name>CLB</name>
-    <debug>false</debug>
-    <enable>
-	  <condition>
-		<and>
-		  <equals>
-			<property>/it-autoflight/output/thr-mode</property>
-			<value>2</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/athr</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/retard</property>
-			<value>0</value>
-		  </equals>
-		</and>
-	  </condition>
-    </enable>
-    <input>
-      <property>/velocities/airspeed-kt</property>
-    </input>
-    <reference>
-      <value>900</value>
-    </reference>
-    <output>
-      <property>/controls/flight/throttle-cmd</property>
-    </output>
-    <config>
-      <Kp>0.1</Kp>        
-      <beta>1.0</beta>    
-      <alpha>0.1</alpha>  
-      <gamma>0.0</gamma>  
-      <Ti>10.0</Ti>       
-      <Td>0.00001</Td>    
-      <u_min>0.0</u_min>  
-      <u_max>0.95</u_max>  
-    </config>
-  </pid-controller>
-  
-<!-- RETARD THR -->
- <pid-controller>
-    <name>RETARD</name>
-    <debug>false</debug>
-    <enable>
-	  <condition>
-		<and>
-		  <equals>
-			<property>/it-autoflight/output/athr</property>
-			<value>1</value>
-		  </equals>
-		  <equals>
-			<property>/it-autoflight/output/retard</property>
-			<value>1</value>
-		  </equals>
-		</and>
-	  </condition>
-    </enable>
-    <input>
-      <property>/velocities/airspeed-kt</property>
-    </input>
-    <reference>
-      <value>0</value>
-    </reference>
-    <output>
-      <property>/controls/flight/throttle-cmd</property>
-    </output>
-    <config>
-      <Kp>0.1</Kp>        
-      <beta>1.0</beta>    
-      <alpha>0.1</alpha>  
-      <gamma>0.0</gamma>  
-      <Ti>10.0</Ti>       
-      <Td>0.00001</Td>    
-      <u_min>0.005</u_min>  
-      <u_max>0.2</u_max>  
-    </config>
-  </pid-controller>
+		</input>
+		<reference>
+			<value>900</value>
+		</reference>
+		<output>
+			<property>/controls/flight/throttle-cmd</property>
+		</output>
+		<config>
+			<Kp>0.1</Kp>        
+			<beta>1.0</beta>    
+			<alpha>0.1</alpha>  
+			<gamma>0.0</gamma>  
+			<Ti>10.0</Ti>       
+			<Td>0.00001</Td>    
+			<u_min>0.0</u_min>  
+			<u_max>0.95</u_max>  
+		</config>
+	</pid-controller>
   
 	<filter>
 		<name>IT-CONTROLLER: THRUST CMD</name>
@@ -244,6 +224,7 @@
 			</condition>
 		</enable>
 		<input>
+			
 			<property>/controls/flight/throttle-cmd</property>
 		</input>
 		<output>
@@ -270,7 +251,7 @@
 					</equals>
 				</and>
 			</condition>
-			<value>0.40</value>
+			<value>0.20</value>
 		</max-rate-of-change>
 		<max-rate-of-change>
 			<condition>
@@ -285,7 +266,7 @@
 					</equals>
 				</and>
 			</condition>
-			<value>0.20</value>
+			<value>0.05</value>
 		</max-rate-of-change>
 		<max-rate-of-change>
 			<condition>
@@ -294,7 +275,7 @@
 					<value>1</value>
 				</equals>
 			</condition>
-			<value>0.20</value>
+			<value>0.10</value>
 		</max-rate-of-change>
 	</filter>