diff --git a/Nasal/FMGC/FMGC-b.nas b/Nasal/FMGC/FMGC-b.nas
index 63403ffb..9da99042 100644
--- a/Nasal/FMGC/FMGC-b.nas
+++ b/Nasal/FMGC/FMGC-b.nas
@@ -925,6 +925,7 @@ var ITAF = {
 		} else if (Input.hdgCalc < 1) { # It's rounded, so this is ok. Otherwise do < 0.5
 			Input.hdgCalc = Input.hdgCalc + 360;
 		}
+		updateFma.lat();
 		Input.hdg.setValue(Input.hdgCalc);
 	},
 	trkFpaOff: func() {
@@ -941,6 +942,7 @@ var ITAF = {
 		} else if (Input.hdgCalc < 1) { # It's rounded, so this is ok. Otherwise do < 0.5
 			Input.hdgCalc = Input.hdgCalc + 360;
 		}
+		updateFma.lat();
 		Input.hdg.setValue(Input.hdgCalc);
 	},
 	updateLatText: func(t) {
diff --git a/Nasal/FMGC/FMGC-c.nas b/Nasal/FMGC/FMGC-c.nas
index 1e0aea6e..a734ffa2 100644
--- a/Nasal/FMGC/FMGC-c.nas
+++ b/Nasal/FMGC/FMGC-c.nas
@@ -204,18 +204,6 @@ var loopFMA = maketimer(0.05, func {
 		}
 	}
 	
-	trk = Custom.trkFpa.getValue();
-	latText = Text.lat.getValue();
-	if (latText == "HDG" and trk == 0) {
-		if (newlat != "HDG") {
-			Modes.PFD.FMA.rollMode.setValue("HDG");
-		}
-	} else if (latText == "HDG" and trk == 1) {
-		if (newlat != "TRACK") {
-			Modes.PFD.FMA.rollMode.setValue("TRACK");
-		}
-	}
-	
 	# Boxes
 	elapsedtime = pts.Sim.Time.elapsedSec.getValue();
 	if (Modes.PFD.FMA.apModeTime.getValue() + 10 >= elapsedtime) {
@@ -283,7 +271,17 @@ var updateFma = {
 	lat: func() {
 		latText = Text.lat.getValue();
 		newlat = Modes.PFD.FMA.rollMode.getValue();
-		if (latText == "LNAV") {
+		if (latText == "HDG") {
+			if (Custom.trkFpa.getValue()) {
+				if (newlat != "TRACK") {
+					Modes.PFD.FMA.rollMode.setValue("TRACK");
+				}
+			} else {
+				if (newlat != "HDG") {
+					Modes.PFD.FMA.rollMode.setValue("HDG");
+				}
+			}
+		} else if (latText == "LNAV") {
 			if (newlat != "NAV") {
 				Modes.PFD.FMA.rollMode.setValue("NAV");
 			}
@@ -628,7 +626,7 @@ setlistener("/modes/pfd/fma/pitch-mode-armed", func {
 }, 0, 0);
 
 setlistener("/modes/pfd/fma/pitch-mode2-armed", func {
-	if (Modes.PFD.FMA.pitchMode2Armed != " ") {
+	if (Modes.PFD.FMA.pitchMode2Armed.getValue() != " ") {
 		Modes.PFD.FMA.pitchMode2ArmedTime.setValue(pts.Sim.Time.elapsedSec.getValue());
 	}
 }, 0, 0);