diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index dd919b4fbc..1374624fa7 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -190,11 +190,17 @@ static void calc_nav_pitch()
 
 static void calc_nav_roll()
 {
-	float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
+	//float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
 	
-	nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
+	//nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
 	
 	//printf("nvrl %ld err %ld psi %f gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
+	
+	long psi_dot_cmd = g.pidNavRoll.kP() * bearing_error;
+	
+	nav_roll = (100 * ToDeg(atan((psi_dot_cmd * g_gps->ground_speed) / 9810000.0)));
+	
+//	printf("nvrl %ld err %ld psi %ld gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
 
 	Vector3f omega;
 	omega = dcm.get_gyro();
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 46e8ad7683..a9f7934e74 100644
--- a/ArduPlane/commands.pde
+++ b/ArduPlane/commands.pde
@@ -180,9 +180,9 @@ static void set_next_WP(struct Location *wp)
 	scaleLongDown 		= cos(rads);
 	scaleLongUp 		= 1.0f/cos(rads);	
 	// this is handy for the groundstation
-	wp_totalDistance 	= get_distance(&prev_WP, &next_WP);
+	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
 	wp_distance 		= wp_totalDistance;
-	target_bearing 		= get_bearing(&prev_WP, &next_WP);
+	target_bearing 		= get_bearing(&current_loc, &next_WP);
 	nav_bearing 		= target_bearing;
 
 	// to check if we have missed the WP
diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 6331d689ab..27f9694e1b 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -183,7 +183,7 @@ static void update_crosstrack(void)
 
 static void reset_crosstrack()
 {
-	crosstrack_bearing 	= get_bearing(&current_loc, &next_WP);	// Used for track following
+	crosstrack_bearing 	= get_bearing(&prev_WP, &next_WP);	// Used for track following
 }
 
 static long get_distance(struct Location *loc1, struct Location *loc2)