diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index 7f5b8e05c9..4bf6ebc830 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -137,7 +137,7 @@ static void set_next_WP(struct Location *wp)
 	if (next_WP.lat == 0 || command_nav_index <= 1){
 		prev_WP = current_loc;
 	}else{
-		if (get_distance(&current_loc, &next_WP) < 10)
+		if (get_distance(&current_loc, &next_WP) < 250)
 			prev_WP = next_WP;
 		else
 			prev_WP = current_loc;
@@ -167,7 +167,6 @@ static void set_next_WP(struct Location *wp)
 	// -----------------------------------
 	wp_distance 		= get_distance(&current_loc, &next_WP);
 	target_bearing 		= get_bearing(&prev_WP, &next_WP);
-	nav_bearing 		= target_bearing;
 
 	// calc the location error:
 	calc_location_error(&next_WP);