diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index d7bd485f4a..e7b885b642 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -649,11 +649,15 @@ void slow_loop()
 		case 0:
 			slow_loopCounter++;
 			superslow_loopCounter++;
-			if(superslow_loopCounter >=15) {
+			if(superslow_loopCounter >= 15) {
 				// keep track of what page is in use in the log
 				// *** We need to come up with a better scheme to handle this...
 				eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
 				superslow_loopCounter = 0;
+
+				// save current data to the flash
+				if (log_bitmask & MASK_LOG_CUR)
+					Log_Write_Current();
 			}
 			break;
 			
@@ -682,9 +686,6 @@ void slow_loop()
 			slow_loopCounter = 0;
 			update_events();
 			
-			// save current data to the flash
-			if (log_bitmask & MASK_LOG_CUR)
-				Log_Write_Current();
 
 			break;
 
@@ -868,17 +869,16 @@ void update_current_flight_mode(void)
 						dTnav = 200;
 					}
 					
-					next_WP.lat = home.lat + rc_1.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
-					next_WP.lng = home.lng - rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
+					next_WP.lat = home.lat + rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
+					next_WP.lng = home.lng + rc_1.control_in / 5; // 4500 / 5 = 900 = 10 meteres 
 				}
 
-				// Yaw control
-				// -----------
-				output_manual_yaw();
-				
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 
+				// Yaw control
+				output_manual_yaw();
+				
 				// apply throttle control
 				output_manual_throttle();
 
@@ -897,6 +897,7 @@ void update_current_flight_mode(void)
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
 				
+				//if(rc_3.control_in)
 				// get desired height from the throttle
 				next_WP.alt 	= home.alt + (rc_3.control_in * 4); // 0 - 1000 (40 meters)
 				
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 545dbe5525..2e0131392b 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -363,7 +363,7 @@
 // Navigation control gains
 //
 #ifndef NAV_P
-# define NAV_P					1.2
+# define NAV_P					2.4
 #endif
 #ifndef NAV_I
 # define NAV_I					0.01
@@ -386,7 +386,7 @@
 # define THROTTLE_BARO_I		0.04
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.1
+# define THROTTLE_BARO_D		0.15
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		100
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 8204edbb10..1651abba26 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -214,6 +214,8 @@ set_servos_4()
 			//Serial.print("-");
 		}
 		
+		reset_I();
+		
 		if(rc_3.control_in > 0){
 			// we have pushed up the throttle
 			// remove safety
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 84e01be113..52b912ccee 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -69,9 +69,11 @@ void calc_nav()
 	nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
 
 	// rotate the vector
-	nav_roll 	= (float)nav_lat * yawvector.x - (float)nav_lon * yawvector.y;
-	nav_pitch 	= (float)nav_lon * yawvector.x + (float)nav_lat * yawvector.y;
-	//Serial.printf("vx %4.4f,vy %4.4f, nr %ld, np %ld ", yawvector.x, yawvector.y, nav_roll, nav_pitch);
+	nav_roll 	= (float)nav_lon * yawvector.x + (float)nav_lat * yawvector.y;
+	nav_pitch 	= (float)nav_lat * yawvector.x + (float)nav_lon * yawvector.y;
+	
+	//Serial.printf("vx %4.4f,vy %4.4f ", yawvector.x, yawvector.y);
+	
 	nav_roll 	= constrain(nav_roll,  -pitch_max, pitch_max);
 	nav_pitch 	= constrain(nav_pitch, -pitch_max, pitch_max);
 }
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 6f59f1f617..d580d0fa7d 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -361,8 +361,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
 					next_WP.lng,
 					nav_lat,
 					nav_lon,
-					nav_pitch,
 					nav_roll,
+					nav_pitch,
 					pitch_max);
 					
 				print_motor_out();