diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index 8564690453..dd919b4fbc 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -190,18 +190,11 @@ static void calc_nav_pitch()
 
 static void calc_nav_roll()
 {
-
-	// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
-	// This does not make provisions for wind speed in excess of airframe speed
-	nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
-	nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
-
-	// negative error = left turn
-	// positive error = right turn
-	// Calculate the required roll of the plane
-	// ----------------------------------------
-	nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler);	//returns desired bank angle in degrees*100
-	nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
+	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)));
+	
+	//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);
 
 	Vector3f omega;
 	omega = dcm.get_gyro();
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index a9f7934e74..46e8ad7683 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(&current_loc, &next_WP);
+	wp_totalDistance 	= get_distance(&prev_WP, &next_WP);
 	wp_distance 		= wp_totalDistance;
-	target_bearing 		= get_bearing(&current_loc, &next_WP);
+	target_bearing 		= get_bearing(&prev_WP, &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 2f1e803c7a..6331d689ab 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -75,15 +75,15 @@ static void calc_airspeed_errors()
 }
 
 static void calc_bearing_error()
-{
+{/*
 	if(takeoff_complete == true  || g.compass_enabled == true) {
 		bearing_error = nav_bearing - dcm.yaw_sensor;
 	} else {
-
+*/
 		// TODO: we need to use the Yaw gyro for in between GPS reads,
 		// maybe as an offset from a saved gryo value.
 		bearing_error = nav_bearing - g_gps->ground_course;
-	}
+//	}
 
 	bearing_error = wrap_180(bearing_error);
 }