diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index d9d536aeab..a6b11cde04 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -1139,6 +1139,9 @@ void update_navigation()
 			calc_loiter_nav();
 
 		} else {
+
+			update_crosstrack();
+
 			// calc a rate dampened pitch to the target
 			calc_rate_nav();
 
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index bab6eb45b2..2ffa1e2b20 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -175,10 +175,10 @@ void calc_nav_throttle()
 
 	if(altitude_sensor == BARO){
 		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .25
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 120);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 140);
 	}else{
 		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .5
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 150);
 	}
 
 	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 982f437270..135c81ce01 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -361,9 +361,7 @@ void Log_Read_Nav_Tuning()
 								"%d, %d, "
 								"%d, %d, %d, %d, "
 								"%d, %d, %d, "
-								"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"
-
-								),
+								"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"),
 				DataFlash.ReadInt(),	//yaw sensor
 				DataFlash.ReadInt(),	//distance
 				DataFlash.ReadByte(),	//bitmask
diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index b1be9f6df2..05a743d65b 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -2,6 +2,7 @@
 
 void init_commands()
 {
+	// zero is home, but we always load the next command (1), in the code.
     g.waypoint_index.set_and_save(0);
 
     // This are registers for the current may and must commands
@@ -15,10 +16,10 @@ void init_commands()
 
 void init_auto()
 {
-	if (g.waypoint_index == g.waypoint_total) {
-		Serial.println("ia_f");
-		do_RTL();
-	}
+	//if (g.waypoint_index == g.waypoint_total) {
+	//	Serial.println("ia_f");
+	//	do_RTL();
+	//}
 
 	// initialize commands
 	// -------------------
@@ -28,7 +29,7 @@ void init_auto()
 // forces the loading of a new command
 // queue is emptied after a new command is processed
 void clear_command_queue(){
-	next_command.id 	= CMD_BLANK;
+	next_command.id 	= NO_COMMAND;
 }
 
 // Getters
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index 0c71af4b49..c713649a96 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/ArduCopterMega/commands_logic.pde
@@ -374,14 +374,11 @@ bool verify_land()
 	//Serial.printf("N, %d\n", velocity_land);
 	//Serial.printf("N_alt, %ld\n", next_WP.alt);
 
-	//update_crosstrack();
 	return false;
 }
 
 bool verify_nav_wp()
 {
-	update_crosstrack();
-
 	// Altitude checking
 	if(next_WP.options & WP_OPTION_ALT_REQUIRED){
 		// we desire a certain minimum altitude
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index 72af331fba..0a04f9a505 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/ArduCopterMega/commands_process.pde
@@ -24,8 +24,7 @@ void update_commands(void)
 	// This function loads commands into three buffers
 	// when a new command is loaded, it is processed with process_XXX()
 
-	// If we have a command in the queue,
-	// nothing to do.
+	// If we have a command in the queue already
 	if(next_command.id != NO_COMMAND){
 		return;
 	}
@@ -55,7 +54,11 @@ void update_commands(void)
 		increment_WP_index();
 
 		// act on our new command
-		process_next_command();
+		if (process_next_command()){
+			// invalidate command queue so a new one is loaded
+			// -----------------------------------------------
+			clear_command_queue();
+		}
 
 	}
 }
@@ -73,7 +76,8 @@ void verify_commands(void)
 	}
 }
 
-void process_next_command()
+bool
+process_next_command()
 {
 	// these are Navigation/Must commands
 	// ---------------------------------
@@ -95,12 +99,13 @@ void process_next_command()
 
 			// Act on the new command
 			process_must();
+			return true;
 		}
 	}
 
 	// these are Condition/May commands
 	// ----------------------
-	if (command_may_index == 0){
+	if (command_may_index == NO_COMMAND){
 		if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
 
 			// we remember the index of our mission here
@@ -115,6 +120,7 @@ void process_next_command()
 				Log_Write_Cmd(g.waypoint_index, &next_command);
 
 			process_may();
+			return true;
 		}
 
 		// these are Do/Now commands
@@ -126,8 +132,11 @@ void process_next_command()
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
 			process_now();
+			return true;
 		}
 	}
+	// we did not need any new commands
+	return false;
 }
 
 /**************************************************/
@@ -150,9 +159,6 @@ void process_must()
 	// implements the Flight Logic
 	handle_process_must();
 
-	// invalidate command queue so a new one is loaded
-	// -----------------------------------------------
-	clear_command_queue();
 }
 
 void process_may()
@@ -163,19 +169,10 @@ void process_may()
 
 	command_may_ID = next_command.id;
 	handle_process_may();
-
-	// invalidate command queue so a new one is loaded
-	// -----------------------------------------------
-	clear_command_queue();
 }
 
 void process_now()
 {
 	Serial.print("pnow");
 	handle_process_now();
-
-	// invalidate command queue so a new one is loaded
-	// -----------------------------------------------
-	clear_command_queue();
 }
-
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index c7a34745aa..1bed2f2765 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -406,13 +406,13 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_BARO_P
-# define THROTTLE_BARO_P		0.4
+# define THROTTLE_BARO_P		0.3
 #endif
 #ifndef THROTTLE_BARO_I
 # define THROTTLE_BARO_I		0.1
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.1
+# define THROTTLE_BARO_D		0.01
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		50
@@ -420,13 +420,13 @@
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		.8		// upped from .5
+# define THROTTLE_SONAR_P		0.8		// upped from .5
 #endif
 #ifndef THROTTLE_SONAR_I
 # define THROTTLE_SONAR_I		0.1
 #endif
 #ifndef THROTTLE_SONAR_D
-# define THROTTLE_SONAR_D		0.1
+# define THROTTLE_SONAR_D		0.05
 #endif
 #ifndef THROTTLE_SONAR_IMAX
 # define THROTTLE_SONAR_IMAX	60
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 452737ff70..96236c8369 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -94,11 +94,13 @@ set_servos_4()
 			int roll_out 	 	= g.rc_1.pwm_out * .707;
 			int pitch_out 	 	= g.rc_2.pwm_out * .707;
 
-			motor_out[CH_3]	 	= g.rc_3.radio_out + roll_out + pitch_out;
-			motor_out[CH_2]	 	= g.rc_3.radio_out + roll_out - pitch_out;
+			// left
+			motor_out[CH_3]	 	= g.rc_3.radio_out + roll_out + pitch_out;	// FRONT
+			motor_out[CH_2]	 	= g.rc_3.radio_out + roll_out - pitch_out;	// BACK
 
-			motor_out[CH_1]		= g.rc_3.radio_out - roll_out + pitch_out;
-			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out - pitch_out;
+			// right
+			motor_out[CH_1]		= g.rc_3.radio_out - roll_out + pitch_out;  // FRONT
+			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out - pitch_out;	// BACK
 
 			//Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
 
@@ -163,14 +165,14 @@ set_servos_4()
 			int pitch_out 		= (float)g.rc_2.pwm_out * .5;
 
 			//Back side
-			motor_out[CH_8]		= g.rc_3.radio_out - g.rc_2.pwm_out;		// CCW
-			motor_out[CH_1]		= g.rc_3.radio_out + roll_out - pitch_out;	// CW
-			motor_out[CH_3]          = g.rc_3.radio_out - roll_out - pitch_out;	// CW
+			motor_out[CH_8]		= g.rc_3.radio_out - g.rc_2.pwm_out;		// CCW	BACK
+			motor_out[CH_1]		= g.rc_3.radio_out + roll_out - pitch_out;	// CW, 	BACK LEFT
+			motor_out[CH_3]		= g.rc_3.radio_out - roll_out - pitch_out;	// CW	BACK RIGHT
 
 			//Front side
-			motor_out[CH_7]		= g.rc_3.radio_out + g.rc_2.pwm_out;		// CW
-                        motor_out[CH_2] 	= g.rc_3.radio_out + roll_out + pitch_out;	// CCW
-			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW
+			motor_out[CH_7]		= g.rc_3.radio_out + g.rc_2.pwm_out;		// CW	 FRONT
+			motor_out[CH_2] 	= g.rc_3.radio_out + roll_out + pitch_out;	// CCW	 FRONT LEFT
+			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW	 FRONT RIGHT
 
 			motor_out[CH_8]		+= g.rc_4.pwm_out;	// CCW
 			motor_out[CH_2]		+= g.rc_4.pwm_out;	// CCW
@@ -178,7 +180,7 @@ set_servos_4()
 
 			motor_out[CH_1]		-= g.rc_4.pwm_out;	// CW
 			motor_out[CH_7]		-= g.rc_4.pwm_out;	// CW
-			motor_out[CH_3]		-= g.rc_4.pwm_out;      // CW
+			motor_out[CH_3]		-= g.rc_4.pwm_out;  // CW
 
 		}else if (g.frame_type == Y6_FRAME) {
 			//Serial.println("Y6_FRAME");
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index 6145f61106..e1e7b8b997 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -35,7 +35,7 @@ void init_rc_in()
 		g.rc_6.set_range(0,300);
 
 	#elif CHANNEL_6_TUNING == CH6_BARO_KP
-		g.rc_6.set_range(0,500);
+		g.rc_6.set_range(0,800);
 
 	#elif CHANNEL_6_TUNING == CH6_BARO_KD
 		g.rc_6.set_range(0,500);
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 2490fb930b..308a1ccf4f 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.3 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.4 Beta", main_menu_commands);
 
 void init_ardupilot()
 {