diff --git a/Nasal/FMGC-b.nas b/Nasal/FMGC-b.nas
index 762dfc26..72f53a4a 100644
--- a/Nasal/FMGC-b.nas
+++ b/Nasal/FMGC-b.nas
@@ -80,6 +80,7 @@ var APinit = func {
 	setprop("/it-autoflight/internal/heading-error-deg", 0);
 	trkfpa_off();
 	ap_varioust.start();
+	lnavCalc.start();
 	thrustmode();
 }
 
@@ -550,6 +551,39 @@ var ap_various = func {
 	}
 }
 
+var lnavCalc = maketimer(0.05, func {
+	var f = flightplan();
+	var total_distance = getprop("/autopilot/route-manager/total-distance");
+	var distance = total_distance - getprop("/autopilot/route-manager/distance-remaining-nm");
+	var destination_elevation = getprop("/autopilot/route-manager/destination/field-elevation-ft");
+	var leg = f.currentWP();
+	if (leg == nil) {
+		setprop("/it-autoflight/input/lat", 3);
+		return;
+	}
+	var groundspeed = getprop("/velocities/groundspeed-kt");
+	var log_distance = getprop("/instrumentation/gps/wp/wp[1]/distance-nm");
+	var along_route = total_distance - getprop("/autopilot/route-manager/distance-remaining-nm");
+	var geocoord = geo.aircraft_position();
+	var referenceCourse = f.pathGeod(f.indexOfWP(f.destination_runway), -getprop("/autopilot/route-manager/distance-remaining-nm"));
+	var courseCoord = geo.Coord.new().set_latlon(referenceCourse.lat, referenceCourse.lon);
+	var CourseError = (geocoord.distance_to(courseCoord) / 1852) + 1;
+	var change_wp = abs(getprop("/autopilot/route-manager/wp/bearing-deg") - getprop("/orientation/heading-deg"));
+	if (change_wp > 180) {
+		change_wp = (360 - change_wp);
+	}
+	CourseError += (change_wp / 20);
+	var targetCourse = f.pathGeod(f.indexOfWP(f.destination_runway), (-getprop("autopilot/route-manager/distance-remaining-nm") + CourseError));
+	courseCoord = geo.Coord.new().set_latlon(targetCourse.lat, targetCourse.lon);
+	CourseError = (geocoord.course_to(courseCoord) - getprop("/orientation/heading-deg"));
+	if (CourseError < -180) {
+		CourseError += 360;
+	} else if (CourseError > 180) {
+		CourseError -= 360;
+	}
+	setprop("/it-autoflight/internal/lnav-error-deg", CourseError);
+});
+
 var flch_on = func {
 	setprop("/it-autoflight/output/appr-armed", 0);
 	setprop("/it-autoflight/output/vert", 4);
diff --git a/Systems/fmgc-drivers.xml b/Systems/fmgc-drivers.xml
index d0caa9eb..65ef60a4 100644
--- a/Systems/fmgc-drivers.xml
+++ b/Systems/fmgc-drivers.xml
@@ -487,11 +487,7 @@
 					<value>1</value>
 				</equals>
 			</condition>
-			<property>/autopilot/settings/true-heading-deg</property>
-			<offset>
-				<property>/orientation/heading-deg</property>
-				<scale>-1.0</scale>
-			</offset>
+			<property>/it-autoflight/internal/lnav-error-deg</property>
 		</input>
 		<input>
 			<condition>
diff --git a/revision.txt b/revision.txt
index eb888846..f9e0edcb 100644
--- a/revision.txt
+++ b/revision.txt
@@ -1 +1 @@
-4009
\ No newline at end of file
+4010
\ No newline at end of file