diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 407977c202..dc9c4256d1 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -43,12 +43,12 @@
 
 # define LOG_ATTITUDE_FAST		DISABLED
 # define LOG_ATTITUDE_MED 		DISABLED
-# define LOG_GPS 				DISABLED
+# define LOG_GPS 				ENABLED
 # define LOG_PM 				DISABLED
 # define LOG_CTUN				ENABLED
-# define LOG_NTUN				DISABLED
-# define LOG_MODE				DISABLED
+# define LOG_NTUN				ENABLED
+# define LOG_MODE				ENABLED
 # define LOG_RAW				DISABLED
-# define LOG_CMD				DISABLED
+# define LOG_CMD				ENABLED
 # define LOG_CURRENT			DISABLED
 
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 4d8345901f..bf3e6f0b21 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -256,6 +256,8 @@ long	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of plane to t
 int		climb_rate;							// m/s * 100  - For future implementation of controlled ascent/descent by rate
 float	nav_gain_scaler 		= 1;		// Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
 
+int 	last_ground_speed;					// used to dampen navigation
+
 byte	command_must_index;					// current command memory location
 byte	command_may_index;					// current command memory location
 byte	command_must_ID;					// current command ID
@@ -310,7 +312,6 @@ int 			ground_temperature;
 // ----------------------
 int		sonar_alt;
 int		baro_alt;
-int		baro_alt_offset;
 byte 	altitude_sensor = BARO;				// used to know which sensor is active, BARO or SONAR
 
 // flight mode specific
@@ -325,10 +326,10 @@ byte 	yaw_tracking = TRACK_NONE;			// no tracking, point at next wp, or at a tar
 
 // Loiter management
 // -----------------
-long 	old_target_bearing;					// deg * 100
-int		loiter_total; 						// deg : how many times to loiter * 360
-int 	loiter_delta;						// deg : how far we just turned
-int		loiter_sum;							// deg : how far we have turned around a waypoint
+long 	saved_target_bearing;				// deg * 100
+//int		loiter_total; 						// deg : how many times to loiter * 360
+//int 	loiter_delta;						// deg : how far we just turned
+//int		loiter_sum;							// deg : how far we have turned around a waypoint
 long 	loiter_time;						// millis : when we started LOITER mode
 long 	loiter_time_max;					// millis : how long to stay in LOITER mode
 
@@ -342,6 +343,8 @@ long	nav_lon;							// for error calcs
 int		nav_throttle;						// 0-1000 for throttle control
 int		nav_throttle_old;					// for filtering
 
+bool 	invalid_throttle;					// used to control when we calculate nav_throttle
+bool 	invalid_nav;						// used to control when we calculate nav_throttle
 bool 	set_throttle_cruise_flag = false;	// used to track the throttle crouse value
 
 long 	command_yaw_start;					// what angle were we to begin with
@@ -591,12 +594,17 @@ void medium_loop()
 
 			// we call these regardless of GPS because of the rapid nature of the yaw sensor
 			// -----------------------------------------------------------------------------
-			if(wp_distance < 800){ // 8 meters
+			//if(wp_distance < 800){ // 8 meters
 			//if(g.rc_6.control_in > 500){ // 8 meters
-				calc_loiter_nav();
-			}else{
-				calc_waypoint_nav();
-			}
+				//calc_loiter_nav();
+			//	calc_waypoint_nav();
+			//}else{
+			//	calc_waypoint_nav();
+			//}
+			invalid_nav = true;
+
+			// replace with invalidater byte
+			//calc_waypoint_nav();
 
 			break;
 
@@ -609,10 +617,22 @@ void medium_loop()
 			// --------------------------
 			update_alt();
 
+			// altitude smoothing
+			// ------------------
+			//calc_altitude_smoothing_error();
+
+			calc_altitude_error();
+
+			// invalidate the throttle hold value
+			// ----------------------------------
+			invalid_throttle = true;
+
 			// perform next command
 			// --------------------
 			if(control_mode == AUTO || control_mode == GCS_AUTO){
-				update_commands();
+				if(home_is_set){
+					update_commands();
+				}
 			}
 			break;
 
@@ -790,7 +810,7 @@ void super_slow_loop()
 		temp.alt 	= gcs_simple.altitude;
 		temp.lat 	= gcs_simple.latitude;
 		temp.lng 	= gcs_simple.longitude;
-		set_wp_with_index(temp, gcs_simple.index);
+		set_command_with_index(temp, gcs_simple.index);
 		gcs_simple.ack();
 		*/
 	//}
@@ -852,6 +872,12 @@ void update_current_flight_mode(void)
 {
 	if(control_mode == AUTO){
 
+		// this is a hack to prevent run up of the throttle I term for alt hold
+		if(command_must_ID == MAV_CMD_NAV_TAKEOFF){
+			invalid_throttle = (g.rc_3.control_in != 0);
+			// make invalid_throttle false if we are waiting to take off.
+		}
+
 		switch(command_must_ID){
 			//case MAV_CMD_NAV_TAKEOFF:
 			//	break;
@@ -864,6 +890,9 @@ void update_current_flight_mode(void)
 				// ------------------------------------
 				auto_yaw();
 
+				//if(invalid_nav)
+					//calc_waypoint_nav();
+
 				// mix in user control
 				control_nav_mixer();
 
@@ -871,6 +900,9 @@ void update_current_flight_mode(void)
 				output_stabilize_roll();
 				output_stabilize_pitch();
 
+				if(invalid_throttle)
+					calc_nav_throttle();
+
 				// apply throttle control
 				output_auto_throttle();
 				break;
@@ -964,7 +996,9 @@ void update_current_flight_mode(void)
 							bearing_error);
 					*/
 					// get nav_pitch and nav_roll
-					calc_waypoint_nav();
+					calc_simple_nav();
+					calc_nav_output();
+					limit_nav_pitch_roll(4500);
 				}
 
 				// Output Pitch, Roll, Yaw and Throttle
@@ -996,6 +1030,10 @@ void update_current_flight_mode(void)
 
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
+
+				if(invalid_throttle)
+					calc_nav_throttle();
+
 				// apply throttle control
 				output_auto_throttle();
 
@@ -1012,6 +1050,9 @@ void update_current_flight_mode(void)
 				// ------------------------------------
 				auto_yaw();
 
+				if(invalid_throttle)
+					calc_nav_throttle();
+
 				// apply throttle control
 				output_auto_throttle();
 
@@ -1034,6 +1075,9 @@ void update_current_flight_mode(void)
 				// -----------
 				output_manual_yaw();
 
+				if(invalid_throttle)
+					calc_nav_throttle();
+
 				// apply throttle control
 				output_auto_throttle();
 
@@ -1063,6 +1107,16 @@ void update_navigation()
 	if(control_mode == AUTO || control_mode == GCS_AUTO){
 		verify_commands();
 
+		// calc a rate dampened pitch to the target
+		calc_rate_nav();
+
+		// rotate that pitch to the copter frame of reference
+		calc_nav_output();
+
+		//limit our copter pitch - this will change if we go to a fully rate limited approach.
+		limit_nav_pitch_roll(g.pitch_max.get());
+
+		// this tracks a location so the copter is always pointing towards it.
 		if(yaw_tracking & TRACK_TARGET_WP){
 			nav_yaw = get_bearing(&current_loc, &target_WP);
 		}
@@ -1106,6 +1160,8 @@ void update_trig(void){
 
 void update_alt()
 {
+	altitude_sensor = BARO;
+
 #if HIL_MODE == HIL_MODE_ATTITUDE
 	current_loc.alt = g_gps->altitude;
 #else
@@ -1114,40 +1170,36 @@ void update_alt()
 		baro_alt 		= read_barometer();
 
 		//filter out bad sonar reads
-		int temp 		= sonar.read();
+		//int temp 		= sonar.read();
 
-		if(abs(temp - sonar_alt) < 300){
-			sonar_alt = temp;
-		}
+		//if(abs(temp - sonar_alt) < 300){
+		//	sonar_alt = temp;
+		//}
 
-		// correct alt for angle of the sonar
-		sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
+		sonar_alt = sonar.read();
 
 		// output a light to show sonar is working
 		update_sonar_light(sonar_alt > 100);
 
-		// decide which sensor we're usings
-		if(sonar_alt < 500){  // less than 5m or 15 feet
-			altitude_sensor = SONAR;
+		// decide if we're using sonar
+		if (baro_alt < 1200){
 
-			// XXX this is a hack for now. it kills accuracy from GPS reading of altitude and focuses
-			// on altitude above flat ground.
-			baro_alt_offset = sonar_alt - baro_alt;
+			// correct alt for angle of the sonar
+			sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
 
-		}else{
-			altitude_sensor = BARO;
+			if(sonar_alt < 500){
+				altitude_sensor = SONAR;
+			}
 		}
 
 		// calculate our altitude
 		if(altitude_sensor == BARO){
-			current_loc.alt = baro_alt + baro_alt_offset + home.alt;
+			current_loc.alt = baro_alt + home.alt;
 		}else{
 			current_loc.alt = sonar_alt + home.alt;
 		}
 
 	}else{
-
-		altitude_sensor = BARO;
 		baro_alt 		= read_barometer();
 		// no sonar altitude
 		current_loc.alt = baro_alt + home.alt;
@@ -1155,17 +1207,6 @@ void update_alt()
 
 	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
 #endif
-
-	// altitude smoothing
-	// ------------------
-	//calc_altitude_smoothing_error();
-
-
-	calc_altitude_error();
-
-	// Amount of throttle to apply for hovering
-	// ----------------------------------------
-	calc_nav_throttle();
 }
 
 void
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 1afc46ef18..3c4c4eae64 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -13,11 +13,6 @@ init_pids()
 void
 control_nav_mixer()
 {
-	// limit the nav pitch and roll of the copter
-	long pmax = g.pitch_max.get();
-	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
-	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
-
 	// control +- 45° is mixed with the navigation request by the Autopilot
 	// output is in degrees = target pitch and roll of copter
 	g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
@@ -33,6 +28,15 @@ simple_mixer()
 	g.rc_2.servo_out = nav_pitch;
 }
 
+void
+limit_nav_pitch_roll(long pmax)
+{
+	// limit the nav pitch and roll of the copter
+	//long pmax 	= g.pitch_max.get();
+	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
+	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
+}
+
 void
 output_stabilize_roll()
 {
@@ -95,6 +99,130 @@ output_stabilize_pitch()
 	g.rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
 }
 
+void
+output_rate_roll()
+{
+	// rate control
+	long rate		= degrees(omega.x) * 100; 												// 3rad = 17188 , 6rad = 34377
+	rate			= constrain(rate, -36000, 36000);										// limit to something fun!
+	long error		= ((long)g.rc_1.control_in * 8) - rate;									// control is += 4500 * 8 = 36000
+
+	g.rc_1.servo_out 	= g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
+	g.rc_1.servo_out 	= constrain(g.rc_1.servo_out, -2400, 2400);							// limit to 2400
+}
+
+void
+output_rate_pitch()
+{
+	// rate control
+	long rate		= degrees(omega.y) * 100; 												// 3rad = 17188 , 6rad = 34377
+	rate			= constrain(rate, -36000, 36000);										// limit to something fun!
+	long error		= ((long)g.rc_2.control_in * 8) - rate;									// control is += 4500 * 8 = 36000
+
+	g.rc_2.servo_out 	= g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
+	g.rc_2.servo_out 	= constrain(g.rc_2.servo_out, -2400, 2400);							// limit to 2400
+}
+
+// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
+// Keeps outdated data out of our calculations
+void
+reset_I(void)
+{
+	// I removed these, they don't seem to be needed.
+	//g.pid_nav_lat.reset_I();
+	//g.pid_nav_lon.reset_I();
+}
+
+
+/*************************************************************
+throttle control
+****************************************************************/
+
+// user input:
+// -----------
+void output_manual_throttle()
+{
+	g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
+}
+
+// Autopilot
+// ---------
+void output_auto_throttle()
+{
+	g.rc_3.servo_out 	= (float)nav_throttle * angle_boost();
+	// make sure we never send a 0 throttle that will cut the motors
+	g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
+}
+
+void calc_nav_throttle()
+{
+	// limit error
+	long error = constrain(altitude_error, -400, 400);
+	float scaler = 1.0;
+
+	if(error < 0){
+		// try and prevent rapid fall
+		scaler = (altitude_sensor == BARO) ? .9 : .9;
+	}
+
+	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);
+	}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 = (nav_throttle + nav_throttle_old) >> 1;
+	nav_throttle_old = nav_throttle;
+
+	invalid_throttle = false;
+
+	//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
+}
+
+float angle_boost()
+{
+	float temp = cos_pitch_x * cos_roll_x;
+	temp = 2.0 - constrain(temp, .5, 1.0);
+	return temp;
+}
+
+/*************************************************************
+yaw control
+****************************************************************/
+
+void output_manual_yaw()
+{
+	if(g.rc_3.control_in == 0){
+		// we want to only call this once
+		if(did_clear_yaw_control == false){
+			clear_yaw_control();
+			did_clear_yaw_control = true;
+		}
+
+	}else{ // motors running
+
+		// Yaw control
+		if(g.rc_4.control_in == 0){
+			output_yaw_with_hold(true); // hold yaw
+		}else{
+			output_yaw_with_hold(false); // rate control yaw
+		}
+
+		did_clear_yaw_control = false;
+	}
+}
+
+void auto_yaw()
+{
+	if(yaw_tracking & TRACK_NEXT_WP){
+		nav_yaw = target_bearing;
+	}
+
+	output_yaw_with_hold(true); // hold yaw
+}
+
 void
 clear_yaw_control()
 {
@@ -175,126 +303,3 @@ output_yaw_with_hold(boolean hold)
 
 	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
-
-void
-output_rate_roll()
-{
-	// rate control
-	long rate		= degrees(omega.x) * 100; 												// 3rad = 17188 , 6rad = 34377
-	rate			= constrain(rate, -36000, 36000);										// limit to something fun!
-	long error		= ((long)g.rc_1.control_in * 8) - rate;									// control is += 4500 * 8 = 36000
-
-	g.rc_1.servo_out 	= g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
-	g.rc_1.servo_out 	= constrain(g.rc_1.servo_out, -2400, 2400);							// limit to 2400
-}
-
-void
-output_rate_pitch()
-{
-	// rate control
-	long rate		= degrees(omega.y) * 100; 												// 3rad = 17188 , 6rad = 34377
-	rate			= constrain(rate, -36000, 36000);										// limit to something fun!
-	long error		= ((long)g.rc_2.control_in * 8) - rate;									// control is += 4500 * 8 = 36000
-
-	g.rc_2.servo_out 	= g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
-	g.rc_2.servo_out 	= constrain(g.rc_2.servo_out, -2400, 2400);							// limit to 2400
-}
-
-// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
-// Keeps outdated data out of our calculations
-void
-reset_I(void)
-{
-	g.pid_nav_lat.reset_I();
-	g.pid_nav_lon.reset_I();
-	g.pid_baro_throttle.reset_I();
-	g.pid_sonar_throttle.reset_I();
-}
-
-
-/*************************************************************
-throttle control
-****************************************************************/
-
-// user input:
-// -----------
-void output_manual_throttle()
-{
-	g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
-}
-
-// Autopilot
-// ---------
-void output_auto_throttle()
-{
-	g.rc_3.servo_out 	= (float)nav_throttle * angle_boost();
-	// make sure we never send a 0 throttle that will cut the motors
-	g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
-}
-
-void calc_nav_throttle()
-{
-	// limit error
-	long error = constrain(altitude_error, -400, 400);
-	float scaler = 1.0;
-
-	if(error < 0){
-		// try and prevent rapid fall
-		scaler = (altitude_sensor == BARO) ? .3 : .3;
-	}
-
-	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, -30, 120);
-	}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 = (nav_throttle + nav_throttle_old) >> 1;
-	nav_throttle_old = nav_throttle;
-
-	//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
-}
-
-float angle_boost()
-{
-	float temp = cos_pitch_x * cos_roll_x;
-	temp = 2.0 - constrain(temp, .5, 1.0);
-	return temp;
-}
-
-/*************************************************************
-yaw control
-****************************************************************/
-
-void output_manual_yaw()
-{
-	if(g.rc_3.control_in == 0){
-		// we want to only call this once
-		if(did_clear_yaw_control == false){
-			clear_yaw_control();
-			did_clear_yaw_control = true;
-		}
-
-	}else{ // motors running
-
-		// Yaw control
-		if(g.rc_4.control_in == 0){
-			output_yaw_with_hold(true); // hold yaw
-		}else{
-			output_yaw_with_hold(false); // rate control yaw
-		}
-
-		did_clear_yaw_control = false;
-	}
-}
-
-void auto_yaw()
-{
-	if(yaw_tracking & TRACK_NEXT_WP){
-		nav_yaw = target_bearing;
-	}
-
-	output_yaw_with_hold(true); // hold yaw
-}
diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index 8269140d24..4a54302b11 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -363,7 +363,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
             // send waypoint
-            tell_command = get_wp_with_index(packet.seq);
+            tell_command = get_command_with_index(packet.seq);
 
             // set frame of waypoint
             uint8_t frame;
@@ -507,7 +507,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             {
                 Location temp;  // XXX this is gross
-                temp = get_wp_with_index(packet.seq);
+                temp = get_command_with_index(packet.seq);
                 set_next_WP(&temp);
             }
 
@@ -631,7 +631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 
 
-            set_wp_with_index(tell_command, packet.seq);
+            set_command_with_index(tell_command, packet.seq);
 
             // update waypoint receiving state machine
             waypoint_timelast_receive = millis();
@@ -939,4 +939,5 @@ GCS_MAVLINK::_queued_send()
 }
 
 #endif
-
+
+
diff --git a/ArduCopterMega/GCS_Standard.pde b/ArduCopterMega/GCS_Standard.pde
index f253103542..f2ce0f4a20 100644
--- a/ArduCopterMega/GCS_Standard.pde
+++ b/ArduCopterMega/GCS_Standard.pde
@@ -202,7 +202,7 @@ void send_message(byte id, long param) {
 		tempint 		= g.waypoint_total;			 		// list length (# of commands in mission)
 		mess_buffer[5] 	= tempint & 0xff;
 		mess_buffer[6] 	= (tempint >> 8) & 0xff;
-		tell_command 	= get_wp_with_index((int)param);
+		tell_command 	= get_command_with_index((int)param);
 		mess_buffer[7]	= tell_command.id;			// command id
 		mess_buffer[8]	= tell_command.p1;			// P1
 		tempint			= tell_command.alt;				// P2
diff --git a/ArduCopterMega/GCS_Xplane.pde b/ArduCopterMega/GCS_Xplane.pde
index 2b74c19144..ff8872bb64 100644
--- a/ArduCopterMega/GCS_Xplane.pde
+++ b/ArduCopterMega/GCS_Xplane.pde
@@ -34,7 +34,7 @@ void send_message(byte id, long param) {
 		break;
 		
 		case MSG_COMMAND:
-			struct Location cmd = get_wp_with_index(param);
+			struct Location cmd = get_command_with_index(param);
 			print_waypoint(&cmd, param);
 		break;
 		
@@ -113,12 +113,12 @@ void print_waypoints()
 	// create a location struct to hold the temp Waypoints for printing
 	//Location tmp;
 	Serial.println("Home: ");
-	struct Location cmd = get_wp_with_index(0);
+	struct Location cmd = get_command_with_index(0);
 	print_waypoint(&cmd, 0);
 	Serial.println("Commands: ");
 	
 	for (int i=1; i < g.waypoint_total; i++){
-		cmd = get_wp_with_index(i);
+		cmd = get_command_with_index(i);
 		print_waypoint(&cmd, i);
 	}
 }
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 02d297dcf7..84534edd6f 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -385,13 +385,16 @@ void Log_Write_Nav_Tuning()
 	DataFlash.WriteInt((int)sonar_alt);					// 7
 	DataFlash.WriteInt((int)baro_alt);					// 8
 
-	DataFlash.WriteInt(home.alt);						// 9
-	DataFlash.WriteInt((int)next_WP.alt);				// 11
+	DataFlash.WriteInt((int)home.alt);					// 9
+	DataFlash.WriteInt((int)next_WP.alt);				// 10
 	DataFlash.WriteInt((int)altitude_error);			// 11
 
 	DataFlash.WriteByte(END_BYTE);
 }
 
+    //	1	2	3	4  5  6	 7	 8	   9	  10   11
+//NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
+
 void Log_Read_Nav_Tuning()
 {
 							 //	1	2	3	4	5	6	7	8	9	10  11
@@ -446,8 +449,8 @@ void Log_Write_GPS()
 
 // Read a GPS packet
 void Log_Read_GPS()
-{						//	1	2	3 4				5			6		 7			8        9
-					//	GPS, t, 1, 8, 37.7659070, -122.4329400, 57.0500, 58.1400, 658.8400, -11636846.0000
+{						//	1		 2	3  4			5			 6		   7	   8       9
+					//GPS, 77361250, 1, 9, 40.0584750, -105.2034500, 166.2600, 2.8100, 0.0600, 266.0000
 							//	1   2   3   4      5		6	 7		 8		9
 	Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
 
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index 436f05cde4..29c3a9639a 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -17,7 +17,7 @@ public:
     // The increment will prevent old parameters from being used incorrectly
     // by newer code.
     //
-    static const uint16_t k_format_version = 4;
+    static const uint16_t k_format_version = 5;
 
     //
     // Parameter identities.
@@ -117,6 +117,7 @@ public:
 		k_param_pid_yaw,
 		k_param_pid_nav_lat,
 		k_param_pid_nav_lon,
+		k_param_pid_nav_wp,
 		k_param_pid_baro_throttle,
 		k_param_pid_sonar_throttle,
 		// special D term alternatives
@@ -202,6 +203,7 @@ public:
 	PID			pid_yaw;
 	PID			pid_nav_lat;
 	PID			pid_nav_lon;
+	PID			pid_nav_wp;
 	PID			pid_baro_throttle;
 	PID			pid_sonar_throttle;
 
@@ -272,11 +274,12 @@ public:
 		pid_stabilize_pitch	(k_param_pid_stabilize_pitch,	PSTR("STB_PIT_"),	STABILIZE_PITCH_P,  STABILIZE_PITCH_I,	0,  				STABILIZE_PITCH_IMAX * 100),
 		pid_yaw				(k_param_pid_yaw,				PSTR("STB_YAW_"),	YAW_P,      		YAW_I,				0,					YAW_IMAX * 100),
 
-		pid_nav_lat			(k_param_pid_nav_lat,			PSTR("NAV_LAT_"),	NAV_P,      		NAV_I,				NAV_D,				NAV_IMAX * 100),
-		pid_nav_lon			(k_param_pid_nav_lon,			PSTR("NAV_LON_"),	NAV_P,      		NAV_I,				NAV_D,				NAV_IMAX * 100),
+		pid_nav_lat			(k_param_pid_nav_lat,			PSTR("NAV_LAT_"),	NAV_LOITER_P,		NAV_LOITER_I,		NAV_LOITER_D,		NAV_LOITER_IMAX * 100),
+		pid_nav_lon			(k_param_pid_nav_lon,			PSTR("NAV_LON_"),	NAV_LOITER_P,      	NAV_LOITER_I,		NAV_LOITER_D,		NAV_LOITER_IMAX * 100),
+		pid_nav_wp			(k_param_pid_nav_wp,			PSTR("NAV_WP_"),	NAV_WP_P,      		NAV_WP_I,			NAV_WP_D,			NAV_WP_IMAX * 100),
 
-		pid_baro_throttle	(k_param_pid_baro_throttle,		PSTR("THR_BAR_"),	THROTTLE_BARO_P,    THROTTLE_BARO_I,	THROTTLE_BARO_D,	THROTTLE_BARO_IMAX * 100),
-		pid_sonar_throttle	(k_param_pid_sonar_throttle,	PSTR("THR_SON_"),	THROTTLE_SONAR_P,   THROTTLE_SONAR_I,	THROTTLE_SONAR_D,	THROTTLE_SONAR_IMAX * 100),
+		pid_baro_throttle	(k_param_pid_baro_throttle,		PSTR("THR_BAR_"),	THROTTLE_BARO_P,    THROTTLE_BARO_I,	THROTTLE_BARO_D,	THROTTLE_BARO_IMAX),
+		pid_sonar_throttle	(k_param_pid_sonar_throttle,	PSTR("THR_SON_"),	THROTTLE_SONAR_P,   THROTTLE_SONAR_I,	THROTTLE_SONAR_D,	THROTTLE_SONAR_IMAX),
 
 		stabilize_dampener	(STABILIZE_ROLL_D,		k_param_stabilize_dampener, 	PSTR("STB_DAMP")),
 		hold_yaw_dampener	(YAW_D,				 	k_param_hold_yaw_dampener, 		PSTR("YAW_DAMP")),
diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt
index cfebbbb206..9d2d1a9f52 100644
--- a/ArduCopterMega/command description.txt	
+++ b/ArduCopterMega/command description.txt	
@@ -16,6 +16,9 @@ Command Structure in bytes
 11 0x0C	..
 11 0x0D	..
 
+Update:
+MAV_CMD_NAV_ORIENTATION_TARGET > MAV_CMD_DO_ORIENTATION
+Remove MAV_CMD_NAV_TARGET
 
 
 ***********************************
@@ -28,7 +31,6 @@ Command ID	Name								Parameter 1			Altitude			Latitude			Longitude
 0x14 / 20	MAV_CMD_NAV_RETURN_TO_LAUNCH		-					-					-					-
 0x15 / 21	MAV_CMD_NAV_LAND					-					-					-					-
 0x16 / 22	MAV_CMD_NAV_TAKEOFF					-					altitude			-					-
-0x17 / 23	MAV_CMD_NAV_TARGET					-					altitude			lat					lon
 
 			NOTE:  for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
 
@@ -37,14 +39,12 @@ Command ID	Name								Parameter 1			Altitude			Latitude			Longitude
 May Commands - these commands are optional to finish
 Command ID		Name								Parameter 1			Parameter 2			Parameter 3			Parameter 4
 0x70 / 112		MAV_CMD_CONDITION_DELAY				-					-					time (seconds) -
-
 0x71 / 113		MAV_CMD_CONDITION_CHANGE_ALT 		rate (cm/sec)		alt (finish)		-					-
 													Note: rate must be > 10 cm/sec due to integer math
 
-MAV_CMD_NAV_LAND_OPTIONS			(NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
+//	MAV_CMD_NAV_LAND_OPTIONS			(NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
 
 0x72 / 114		MAV_CMD_CONDITION_DISTANCE			-					-					distance (meters)	-
-
 0x71 / 115		MAV_CMD_CONDITION_YAW				angle				speed				direction (-1,1)	rel (1), abs (0)
 
 ***********************************
@@ -65,14 +65,16 @@ Command ID		Name						Parameter 1			Parameter 2			Parameter 3			Parameter 4
 								(1=use current location, 0=use specified location)
 
 0xB4 / 180		MAV_CMD_DO_SET_PARAMETER	Param number		Param value			(NOT CURRENTLY IMPLEMENTED IN APM)
-
 0xB5 / 181		MAV_CMD_DO_SET_RELAY		Relay number		On/off  (1/0)		-					-
-
 0xB6 / 182		MAV_CMD_DO_REPEAT_RELAY		Relay number		Cycle count			Cycle time (sec)	-
 				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
 
 0xB7 / 183		MAV_CMD_DO_SET_SERVO		Servo number (5-8)	On/off  (1/0)		-					-
-
 0xB6 / 184		MAV_CMD_DO_REPEAT_SERVO		Servo number (5-8)	Cycle count			Cycle time (sec)	-
 				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
 
+// need to add command
+0xB7 / 185		MAV_CMD_DO_ORIENTATION		Yaw_Mode			altitude			lat					lon
+											TRACK_NONE 		1
+											TRACK_NEXT_WP 	2
+											TRACK_TARGET_WP 4
diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index f5af4e697a..5a5233b0c2 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -2,11 +2,15 @@
 
 void init_commands()
 {
-	//read_EEPROM_waypoint_info();
     g.waypoint_index.set_and_save(0);
-	command_must_index	= 0;
-	command_may_index	= 0;
-	next_command.id 	= CMD_BLANK;
+
+    // This are registers for the current may and must commands
+    // setting to zero will allow them to be written to by new commands
+	command_must_index	= NO_COMMAND;
+	command_may_index	= NO_COMMAND;
+
+	// clear the command queue
+	clear_command_queue();
 }
 
 void init_auto()
@@ -21,29 +25,33 @@ void init_auto()
 	init_commands();
 }
 
-// this is only used by an air-start
-/*void reload_commands_airstart()
-{
-	init_commands();
-	g.waypoint_index.load();        // XXX can we assume it's been loaded already by ::load_all?
-	decrement_WP_index();
+// 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;
 }
-*/
 
 // Getters
 // -------
-struct Location get_wp_with_index(int i)
+struct Location get_command_with_index(int i)
 {
 	struct Location temp;
-	long mem;
 
 	// Find out proper location in memory by using the start_byte position + the index
 	// --------------------------------------------------------------------------------
 	if (i > g.waypoint_total) {
+		// we do not have a valid command to load
+		// return a WP with a "Blank" id
 		temp.id = CMD_BLANK;
+
+		// no reason to carry on
+		return temp;
+
 	}else{
+		// we can load a command, we don't process it yet
 		// read WP position
-		mem = (WP_START_BYTE) + (i * WP_SIZE);
+		long mem = (WP_START_BYTE) + (i * WP_SIZE);
+
 		temp.id = eeprom_read_byte((uint8_t*)mem);
 
 		mem++;
@@ -62,30 +70,18 @@ struct Location get_wp_with_index(int i)
 		temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
 	}
 
-	// Add on home altitude if we are a nav command
-	if(temp.id < 0x70){
-		temp.alt += home.alt;
-	}
-
 	if(temp.options & WP_OPTION_RELATIVE){
+		// If were relative, just offset from home
 		temp.lat	+=	home.lat;
 		temp.lng	+=	home.lng;
 	}
 
-	// XXX this is a little awkward. We have two methods to control Yaw tracking
-	// one is global and one is per waypoint.
-	if(temp.options & WP_OPTION_YAW){
-		yaw_tracking = TRACK_NEXT_WP;
-	}
-
-	// this is a hack for now, until we get GUI support
-	yaw_tracking = TRACK_NEXT_WP;
 	return temp;
 }
 
 // Setters
 // -------
-void set_wp_with_index(struct Location temp, int i)
+void set_command_with_index(struct Location temp, int i)
 {
 	i = constrain(i, 0, g.waypoint_total.get());
 	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
@@ -99,13 +95,13 @@ void set_wp_with_index(struct Location temp, int i)
 	eeprom_write_byte((uint8_t *)	mem, temp.p1);
 
 	mem++;
-	eeprom_write_dword((uint32_t *)	mem, temp.alt);	// alt is stored in CM!
+	eeprom_write_dword((uint32_t *)	mem, temp.alt);	// Alt is stored in CM!
 
 	mem += 4;
-	eeprom_write_dword((uint32_t *)	mem, temp.lat);
+	eeprom_write_dword((uint32_t *)	mem, temp.lat);	// Lat is stored in decimal degrees * 10^7
 
 	mem += 4;
-	eeprom_write_dword((uint32_t *)	mem, temp.lng);
+	eeprom_write_dword((uint32_t *)	mem, temp.lng); // Long is stored in decimal degrees * 10^7
 }
 
 void increment_WP_index()
@@ -137,7 +133,8 @@ long read_alt_to_hold()
 
 
 //********************************************************************************
-//This function sets the waypoint and modes for Return to Launch
+// This function sets the waypoint and modes for Return to Launch
+// It's not currently used
 //********************************************************************************
 
 Location get_LOITER_home_wp()
@@ -146,19 +143,19 @@ Location get_LOITER_home_wp()
 	next_WP = current_loc;
 
 	// read home position
-	struct Location temp 	= get_wp_with_index(0);
+	struct Location temp 	= get_command_with_index(0);	// 0 = home
 	temp.id 				= MAV_CMD_NAV_LOITER_UNLIM;
 	temp.alt 				= read_alt_to_hold();
 	return temp;
 }
 
 /*
-This function stores waypoint commands
-It looks to see what the next command type is and finds the last command.
+This function sets the next waypoint command
+It precalculates all the necessary stuff.
 */
+
 void set_next_WP(struct Location *wp)
 {
-	//GCS.send_text_P(SEVERITY_LOW,PSTR("load WP"));
 	SendDebug("MSG <set_next_wp> wp_index: ");
 	SendDebugln(g.waypoint_index, DEC);
 	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@@ -171,14 +168,14 @@ void set_next_WP(struct Location *wp)
 	// ---------------------
 	next_WP = *wp;
 
-	// used to control and limit the rate of climb
-	// -----------------------------------------------
+	// used to control and limit the rate of climb - not used right now!
+	// -----------------------------------------------------------------
 	target_altitude = current_loc.alt;
 
 	// zero out our loiter vals to watch for missed waypoints
-	loiter_delta 		= 0;
-	loiter_sum 			= 0;
-	loiter_total 		= 0;
+	//loiter_delta 		= 0;
+	//loiter_sum 			= 0;
+	//loiter_total 		= 0;
 
 	// this is used to offset the shrinking longitude as we go towards the poles
 	float rads 			= (abs(next_WP.lat)/t7) * 0.0174532925;
@@ -187,13 +184,15 @@ void set_next_WP(struct Location *wp)
 
 	// this is handy for the groundstation
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
+
+	// this makes the data not log for a GPS read
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&current_loc, &next_WP);
 	nav_bearing 		= target_bearing;
 
 	// to check if we have missed the WP
 	// ----------------------------
-	old_target_bearing 	= target_bearing;
+	saved_target_bearing = target_bearing;
 
 	// set a new crosstrack bearing
 	// ----------------------------
@@ -217,19 +216,17 @@ void init_home()
 	home.id 	= MAV_CMD_NAV_WAYPOINT;
 	home.lng 	= g_gps->longitude;				// Lon * 10**7
 	home.lat 	= g_gps->latitude;				// Lat * 10**7
-	home.alt 	= max(g_gps->altitude, 0);
+	home.alt 	= max(g_gps->altitude, 0);		// we sometimes get negatives from GPS, not valid
 	home_is_set = true;
 
 	//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
 
 	// Save Home to EEPROM
 	// -------------------
-	set_wp_with_index(home, 0);
-
+	set_command_with_index(home, 0);
 	print_wp(&home, 0);
 
-	// Save prev loc
-	// -------------
+	// Save prev loc this makes the calcs look better before commands are loaded
 	next_WP = prev_WP = home;
 }
 
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index 0dc449a10a..73d5df347c 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/ArduCopterMega/commands_logic.pde
@@ -5,10 +5,6 @@
 /********************************************************************************/
 void handle_process_must()
 {
-	// reset navigation integrators
-	// -------------------------
-	reset_I();
-
 	switch(next_command.id){
 
 		case MAV_CMD_NAV_TAKEOFF:
@@ -101,23 +97,27 @@ void handle_process_now()
 			do_repeat_relay();
 			break;
 
-		case MAV_CMD_NAV_ORIENTATION_TARGET:
-			do_target_yaw();
+		//case MAV_CMD_DO_ORIENTATION:
+		//	do_target_yaw();
 	}
 }
 
-bool handle_no_commands()
+void handle_no_commands()
 {
+	// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
+	// use landing commands
+	return;
+	/*
 	if (command_must_ID)
-		return false;
+		return;
 
 	switch (control_mode){
-
 		default:
 			//set_mode(RTL);
 			break;
 	}
-	return true;
+	return;
+	*/
 }
 
 /********************************************************************************/
@@ -219,7 +219,9 @@ void do_RTL(void)
 void do_takeoff()
 {
 	Location temp		= current_loc;
-	temp.alt			= next_command.alt;
+
+	// next_command.alt is a relative altitude!!!
+	temp.alt			= next_command.alt + home.alt;
 
 	takeoff_complete	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 
@@ -231,11 +233,21 @@ void do_takeoff()
 
 void do_nav_wp()
 {
-	set_next_WP(&next_command);
-	wp_verify_byte 	= 0;
-	loiter_time 	= 0;
-	loiter_time_max = next_command.p1 * 1000; // units are (seconds)
+	// next_command.alt is a relative altitude!!!
+	next_command.alt += home.alt;
 
+	set_next_WP(&next_command);
+
+	// this is our bitmask to verify we have met all conditions to move on
+	wp_verify_byte 	= 0;
+
+	// this will be used to remember the time in millis after we reach or pass the WP.
+	loiter_time 	= 0;
+
+	// this is the delay, stored in seconds and expanded to millis
+	loiter_time_max = next_command.p1 * 1000;
+
+	// if we don't require an altitude minimum, we save this flag as passed (1)
 	if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){
 		// we don't need to worry about it
 		wp_verify_byte |= NAV_ALTITUDE;
@@ -245,16 +257,20 @@ void do_nav_wp()
 void do_land()
 {
 	Serial.println("dlnd ");
-	land_complete		= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
+
+	// not really used right now, might be good for debugging
+	land_complete		= false;
+
+	// A value that drives to 0 when the altitude doesn't change
 	velocity_land		= 2000;
+
+	// used to limit decent rate
 	land_start 			= millis();
+
+	// used to limit decent rate
 	original_alt		= current_loc.alt;
 
-	//Location temp		= current_loc;
-	//temp.alt			= home.alt;
-	// just go down far
-	//temp.alt			= -100000;
-
+	// hold at our current location
 	set_next_WP(&current_loc);
 }
 
@@ -269,16 +285,19 @@ void do_loiter_unlimited()
 
 void do_loiter_turns()
 {
+/*
 	if(next_command.lat == 0)
 		set_next_WP(&current_loc);
 	else
 		set_next_WP(&next_command);
 
 	loiter_total = next_command.p1 * 360;
+*/
 }
 
 void do_loiter_time()
 {
+	/*
 	if(next_command.lat == 0)
 		set_next_WP(&current_loc);
 	else
@@ -287,6 +306,7 @@ void do_loiter_time()
 	loiter_time 	= millis();
 	loiter_time_max = next_command.p1 * 1000; // units are (seconds)
 	Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
+	*/
 }
 
 /********************************************************************************/
@@ -295,7 +315,7 @@ void do_loiter_time()
 
 bool verify_takeoff()
 {
-	//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
+	Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
 
 	// wait until we are ready!
 	if(g.rc_3.control_in == 0)
@@ -305,7 +325,9 @@ bool verify_takeoff()
 		Serial.println("Y");
 		takeoff_complete = true;
 		return true;
+
 	}else{
+
 		Serial.println("N");
 		return false;
 	}
@@ -314,7 +336,7 @@ bool verify_takeoff()
 bool verify_land()
 {
 	// land at 1 meter per second
-	next_WP.alt  = original_alt - ((millis() - land_start) / 10);			// condition_value = our initial
+	next_WP.alt  = original_alt - ((millis() - land_start) / 5);			// condition_value = our initial
 
 	velocity_land  = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
 	old_alt = current_loc.alt;
@@ -324,15 +346,16 @@ bool verify_land()
 		if(sonar_alt < 40){
 			land_complete = true;
 			Serial.println("Y");
-			return true;
+			//return true;
 		}
 	}
 
 	if(velocity_land <= 0){
 		land_complete = true;
-		return true;
+		//return true;
 	}
-	Serial.printf("N, %d\n", velocity_land);
+	//Serial.printf("N, %d\n", velocity_land);
+	Serial.printf("N_alt, %d\n", next_WP.alt);
 
 	//update_crosstrack();
 	return false;
@@ -351,16 +374,24 @@ bool verify_nav_wp()
 		}
 	}
 
-	// Distance checking
-	if((wp_distance > 0) && (wp_distance <= g.waypoint_radius)){
-		wp_verify_byte |= NAV_LOCATION;
-		if(loiter_time == 0){
-			loiter_time = millis();
+	// Did we pass the WP?	// Distance checking
+	if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
+
+		// if we have a distance calc error, wp_distance may be less than 0
+		if(wp_distance > 0){
+
+			// XXX does this work?
+			wp_verify_byte |= NAV_LOCATION;
+
+			if(loiter_time == 0){
+				loiter_time = millis();
+			}
 		}
 	}
 
-	// Hold at Waypoint checking
-	if(wp_verify_byte & NAV_LOCATION){							// we have reached our goal
+	// Hold at Waypoint checking, we cant move on until this is OK
+	if(wp_verify_byte & NAV_LOCATION){
+		// we have reached our goal
 
 		if ((millis() - loiter_time) > loiter_time_max) {
 			wp_verify_byte |= NAV_DELAY;
@@ -371,7 +402,7 @@ bool verify_nav_wp()
 
 	if(wp_verify_byte == 7){
 		char message[30];
-		sprintf(message,"Reached Waypoint #%i",command_must_index);
+		sprintf(message,"Reached Command #%i",command_must_index);
 		gcs.send_text(SEVERITY_LOW,message);
 		return true;
 	}else{
@@ -579,10 +610,10 @@ void do_jump()
 
 		command_must_index	= 0;
 		command_may_index	= 0;
-		temp				= get_wp_with_index(g.waypoint_index);
+		temp				= get_command_with_index(g.waypoint_index);
 		temp.lat			= next_command.lat - 1;					// Decrement repeat counter
 
-		set_wp_with_index(temp, g.waypoint_index);
+		set_command_with_index(temp, g.waypoint_index);
 		g.waypoint_index.set_and_save(next_command.p1 - 1);
 	}
 }
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index e9f1b4be48..4d0babe51c 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/ArduCopterMega/commands_process.pde
@@ -6,17 +6,41 @@ void update_commands(void)
 {
 	// This function loads commands into three buffers
 	// when a new command is loaded, it is processed with process_XXX()
-	// ----------------------------------------------------------------
-	if(home_is_set == false){
-		return;	// don't do commands
+
+	// If we have a command in the queue,
+	// nothing to do.
+	if(next_command.id != NO_COMMAND){
+		return;
 	}
 
-	if(control_mode == AUTO){
-		load_next_command_from_EEPROM();
+	// fetch next command if the next command queue is empty
+	// -----------------------------------------------------
+	next_command = get_command_with_index(g.waypoint_index + 1);
+
+	if(next_command.id == NO_COMMAND){
+		// if no commands were available from EEPROM
+		// --------------------------------------------
+
+		handle_no_commands();
+		gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
+
+	} else {
+		// A command was loaded from EEPROM
+		// --------------------------------------------
+
+		// debug by outputing the Waypoint loaded
+		print_wp(&next_command, g.waypoint_index + 1);
+
+		// Set our current mission index + 1;
+		increment_WP_index();
+
+		// act on our new command
 		process_next_command();
-	}									// Other (eg GCS_Auto) modes may be implemented here
+
+	}
 }
 
+// called with GPS navigation update - not constantly
 void verify_commands(void)
 {
 	if(verify_must()){
@@ -29,44 +53,27 @@ void verify_commands(void)
 	}
 }
 
-void load_next_command_from_EEPROM()
-{
-	// fetch next command if the next command queue is empty
-	// -----------------------------------------------------
-	if(next_command.id == NO_COMMAND){
-		next_command = get_wp_with_index(g.waypoint_index + 1);
-		//Serial.println("AA");
-		print_wp(&next_command, g.waypoint_index + 1);
-	}
-
-	// If the preload failed, return or just Loiter
-	// --------------------------------------------
-	if(next_command.id == NO_COMMAND){
-		Serial.println("lnc_nc");
-		// we are out of commands!
-		if(handle_no_commands() == true){
-			gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
-		}
-	}
-}
-
 void process_next_command()
 {
 	// these are Navigation/Must commands
 	// ---------------------------------
-	if (command_must_index == 0){ // no current command loaded
+	if (command_must_index == NO_COMMAND){ // no current command loaded
 		if (next_command.id < MAV_CMD_NAV_LAST ){
-			increment_WP_index();
-			//save_command_index();			// TO DO - fix - to Recover from in air Restart
+
+			// we remember the index of our mission here
 			command_must_index = g.waypoint_index;
 
+			// dubugging output
 			SendDebug("MSG <pnc> new c_must_id ");
 			SendDebug(next_command.id,DEC);
 			SendDebug(" index:");
-
 			SendDebugln(command_must_index,DEC);
+
+			// Save CMD to Log
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
+
+			// Act on the new command
 			process_must();
 		}
 	}
@@ -75,23 +82,26 @@ void process_next_command()
 	// ----------------------
 	if (command_may_index == 0){
 		if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
-			increment_WP_index();		// this command is from the command list in EEPROM
+
+			// we remember the index of our mission here
 			command_may_index = g.waypoint_index;
-				SendDebug("MSG <pnc> new may ");
-				SendDebugln(next_command.id,DEC);
-				//Serial.print("new command_may_index ");
-				//Serial.println(command_may_index,DEC);
+			SendDebug("MSG <pnc> new may ");
+			SendDebugln(next_command.id,DEC);
+			//Serial.print("new command_may_index ");
+			//Serial.println(command_may_index,DEC);
+
+			// Save CMD to Log
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
+
 			process_may();
 		}
 
 		// these are Do/Now commands
 		// ---------------------------
 		if (next_command.id > MAV_CMD_CONDITION_LAST){
-			increment_WP_index();		// this command is from the command list in EEPROM
-				SendDebug("MSG <pnc> new now ");
-				SendDebugln(next_command.id,DEC);
+			SendDebug("MSG <pnc> new now ");
+			SendDebugln(next_command.id,DEC);
 
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
@@ -108,18 +118,21 @@ void process_must()
 	//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
 	//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
 	//Serial.printf("pmst %d\n", (int)next_command.id);
-	print_wp(&next_command, g.waypoint_index);
 
-	// clear May indexes
+	// clear May indexes to force loading of more commands
+	// existing May commands are tossed.
 	command_may_index	= NO_COMMAND;
 	command_may_ID		= NO_COMMAND;
 
+	// remember our command ID
 	command_must_ID 	= next_command.id;
+
+	// implements the Flight Logic
 	handle_process_must();
 
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id		= NO_COMMAND;
+	// invalidate command queue so a new one is loaded
+	// -----------------------------------------------
+	clear_command_queue();
 }
 
 void process_may()
@@ -131,9 +144,9 @@ void process_may()
 	command_may_ID = next_command.id;
 	handle_process_may();
 
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id = NO_COMMAND;
+	// invalidate command queue so a new one is loaded
+	// -----------------------------------------------
+	clear_command_queue();
 }
 
 void process_now()
@@ -141,11 +154,8 @@ void process_now()
 	Serial.print("pnow");
 	handle_process_now();
 
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id = NO_COMMAND;
-
-	//gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>"));
-	//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
+	// invalidate command queue so a new one is loaded
+	// -----------------------------------------------
+	clear_command_queue();
 }
 
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 553ac5b628..2db3e390d4 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -360,24 +360,38 @@
 //
 // how much to we pitch towards the target
 #ifndef PITCH_MAX
-# define PITCH_MAX				15			// degrees
+# define PITCH_MAX				45			// degrees
 #endif
 
 
 //////////////////////////////////////////////////////////////////////////////
 // Navigation control gains
 //
-#ifndef NAV_P
-# define NAV_P					2.0
+#ifndef NAV_LOITER_P
+# define NAV_LOITER_P			2.0
 #endif
-#ifndef NAV_I
-# define NAV_I					0.1
+#ifndef NAV_LOITER_I
+# define NAV_LOITER_I			0.1
 #endif
-#ifndef NAV_D
-# define NAV_D					0.00		// should always be 0
+#ifndef NAV_LOITER_D
+# define NAV_LOITER_D			0.00
 #endif
-#ifndef NAV_IMAX
-# define NAV_IMAX				250			// 250 Lat and Longtitude
+#ifndef NAV_LOITER_IMAX
+# define NAV_LOITER_IMAX		250			// 250 Lat and Longtitude
+#endif
+
+
+#ifndef NAV_WP_P
+# define NAV_WP_P				4.0
+#endif
+#ifndef NAV_WP_I
+# define NAV_WP_I				0.0
+#endif
+#ifndef NAV_WP_D
+# define NAV_WP_D				15
+#endif
+#ifndef NAV_WP_IMAX
+# define NAV_WP_IMAX			20			// 20 degrees
 #endif
 
 
@@ -391,24 +405,24 @@
 # define THROTTLE_BARO_I		0.1
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.03	// lowered to 0 to debug effects
+# define THROTTLE_BARO_D		0.1
 #endif
 #ifndef THROTTLE_BARO_IMAX
-# define THROTTLE_BARO_IMAX		50
+# define THROTTLE_BARO_IMAX		80
 #endif
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		.8		// upped a hair from .5
+# define THROTTLE_SONAR_P		.8		// upped from .5
 #endif
 #ifndef THROTTLE_SONAR_I
-# define THROTTLE_SONAR_I		0.4
+# define THROTTLE_SONAR_I		0.1
 #endif
 #ifndef THROTTLE_SONAR_D
-# define THROTTLE_SONAR_D		0.15
+# define THROTTLE_SONAR_D		0.1
 #endif
 #ifndef THROTTLE_SONAR_IMAX
-# define THROTTLE_SONAR_IMAX	50
+# define THROTTLE_SONAR_IMAX	80
 #endif
 
 
diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde
index 14ef06f277..451a8d074c 100644
--- a/ArduCopterMega/control_modes.pde
+++ b/ArduCopterMega/control_modes.pde
@@ -14,7 +14,7 @@ void read_control_switch()
 
 		// reset navigation integrators
 		// -------------------------
-		reset_I();
+		//reset_I();
 	}
 }
 
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 742a46852b..af171c335e 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -319,7 +319,7 @@ set_servos_4()
 
 	}else{
 		// our motor is unarmed, we're on the ground
-		reset_I();
+		//reset_I();
 
 		if(g.rc_3.control_in > 0){
 			// we have pushed up the throttle
@@ -345,7 +345,7 @@ set_servos_4()
 		}
 
 		// reset I terms of PID controls
-		reset_I();
+		//reset_I();
 
 		// Initialize yaw command to actual yaw when throttle is down...
 		g.rc_4.control_in = ToDeg(dcm.yaw);
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 4d1b8e14d5..99cc3c68da 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -35,16 +35,13 @@ void navigate()
 	// ------------------------------------------
 	nav_bearing = target_bearing;
 
-	// check if we have missed the WP
-	loiter_delta = (target_bearing - old_target_bearing)/100;
+}
 
-	// reset the old value
-	old_target_bearing = target_bearing;
-
-	// wrap values
-	if (loiter_delta > 180) loiter_delta -= 360;
-	if (loiter_delta < -180) loiter_delta += 360;
-	loiter_sum += abs(loiter_delta);
+bool check_missed_wp()
+{
+	long temp 	= target_bearing - saved_target_bearing;
+	temp 		= wrap_180(temp);
+	return (abs(temp) > 10000);	//we pased the waypoint by 10 °
 }
 
 #define DIST_ERROR_MAX 1800
@@ -59,6 +56,7 @@ void calc_loiter_nav()
 	10000 	= 111m
 	pitch_max = 22° (2200)
 	*/
+
 	// X ROLL
 	long_error	= (float)(next_WP.lng - current_loc.lng) * scaleLongDown;   // 500 - 0 = 500 roll EAST
 
@@ -77,10 +75,6 @@ void calc_loiter_nav()
 	//nav_lat		= lat_error * g.pid_nav_lat.kP();						// 1800 * 2 = 3600 or 36°
 	nav_lat 	= g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);			// invert lat (for pitch)
 
-
-	// nav_lat  = -1000  Y Pitch
-	// nav_lon  =  1000  X Roll
-
 	// rotate the vector
 	nav_roll 	=   (float)nav_lon * sin_yaw_y 	- (float)nav_lat * -cos_yaw_x;
 					// BAD
@@ -110,13 +104,37 @@ void calc_loiter_nav()
 
 }
 
-void calc_waypoint_nav()
+void calc_simple_nav()
 {
+	// no dampening here in SIMPLE mode
+	nav_lat	= constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
+	// Scale response by kP
+	nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
+}
+
+void calc_rate_nav()
+{
+	// calc distance error
 	nav_lat	= constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
 
 	// Scale response by kP
 	nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
 
+	// Scale response by kP
+	//long output 	= g.pid_nav_wp.kP() * error;
+	int dampening	= g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
+
+	// remember our old speed
+	last_ground_speed = g_gps->ground_speed;
+
+
+	// dampen our response
+	nav_lat -= constrain(dampening, 	-1800, 1800); // +- 20m max error
+
+}
+
+void calc_nav_output()
+{
 	// get the sin and cos of the bearing error - rotated 90°
 	sin_nav_y 	= sin(radians((float)(9000 - bearing_error) / 100));
 	cos_nav_x 	= cos(radians((float)(bearing_error - 9000) / 100));
@@ -124,9 +142,40 @@ void calc_waypoint_nav()
 	// rotate the vector
 	nav_roll 	=  (float)nav_lat * cos_nav_x;
 	nav_pitch 	= -(float)nav_lat * sin_nav_y;
-
 }
 
+#define WAYPOINT_SPEED 450
+
+
+// called after we get GPS read
+void calc_rate_nav2()
+{
+	// change to rate error
+	// we want to be going 450cm/s
+	int error = WAYPOINT_SPEED - g_gps->ground_speed;
+
+	// which direction are we moving?
+	long target_error 	= target_bearing - g_gps->ground_course;
+	target_error 		= wrap_180(target_error);
+
+	// calc the cos of the error to tell how fast we are moving towards the target
+	error = (float)error * cos(radians((float)target_error/100));
+
+	// Scale response by kP
+	long nav_lat 	= g.pid_nav_wp.kP() * error;
+	int dampening	= g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
+
+	// remember our old speed
+	last_ground_speed = g_gps->ground_speed;
+
+	// dampen our response
+	nav_lat -= constrain(dampening, 	-1800, 1800); // +- 20m max error
+
+	// limit our output
+	nav_lat	= constrain(nav_lat, 	-3800, 3800); // +- 20m max error
+}
+
+
 void calc_bearing_error()
 {
 	bearing_error 	= nav_bearing - dcm.yaw_sensor;
@@ -167,34 +216,6 @@ long wrap_180(long error)
 	return error;
 }
 
-void update_loiter()
-{
-	float power;
-
-	if(wp_distance <= g.loiter_radius){
-		power = float(wp_distance) / float(g.loiter_radius);
-		nav_bearing += (int)(9000.0 * (2.0 + power));
-
-	}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
-		power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
-		power = constrain(power, 0, 1);
-		nav_bearing -= power * 9000;
-
-	}else{
-		update_crosstrack();
-		loiter_time = millis();			// keep start time for loiter updating till we get within LOITER_RANGE of orbit
-	}
-
-	if (wp_distance < g.loiter_radius){
-		nav_bearing += 9000;
-	}else{
-		nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
-	}
-
-	update_crosstrack;
-	nav_bearing = wrap_360(nav_bearing);
-}
-
 void update_crosstrack(void)
 {
 	// Crosstrack Error
diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text
index 0708213cac..4ec2b26011 100644
--- a/ArduCopterMega/read_me.text
+++ b/ArduCopterMega/read_me.text
@@ -78,3 +78,8 @@ Auto modes will NOT engage until the throttle is above neutral. So if you put th
 Good luck,
 Jason
 
+
+
+
+TODO:
+Move crosstrack navigation to full PID to limit the velocity towards a waypoint
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 812c004a83..e328e04063 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -700,6 +700,7 @@ void default_log_bitmask()
 void
 default_gains()
 {
+	/*
 	// acro, angular rate
 	g.pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
 	g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
@@ -764,6 +765,7 @@ default_gains()
 	g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
 
 	save_EEPROM_PID();
+	*/
 }
 
 
@@ -775,11 +777,11 @@ void report_wp(byte index = 255)
 {
 	if(index == 255){
 		for(byte i = 0; i <= g.waypoint_total; i++){
-			struct Location temp = get_wp_with_index(i);
+			struct Location temp = get_command_with_index(i);
 			print_wp(&temp, i);
 		}
 	}else{
-		struct Location temp = get_wp_with_index(index);
+		struct Location temp = get_command_with_index(index);
 		print_wp(&temp, index);
 	}
 }
@@ -1185,7 +1187,7 @@ void read_EEPROM_PID(void)
 
 void save_EEPROM_PID(void)
 {
-
+	/*
 	g.pid_acro_rate_roll.save_gains();
 	g.pid_acro_rate_pitch.save_gains();
 	g.pid_acro_rate_yaw.save_gains();
@@ -1203,6 +1205,7 @@ void save_EEPROM_PID(void)
 	g.stabilize_dampener.save();
 	// yaw
 	g.hold_yaw_dampener.save();
+	*/
 }
 
 /********************************************************************************/
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index b7a7261c58..d682a20d3b 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -828,38 +828,38 @@ test_mission(uint8_t argc, const Menu::arg *argv)
 
 	// clear home
 	{Location t = {0,   	0,      0, 		0, 		0, 			0};
-	set_wp_with_index(t,0);}
+	set_command_with_index(t,0);}
 
 	// CMD										opt		pitch   alt/cm
 	{Location t = {MAV_CMD_NAV_TAKEOFF,   		0,      0, 		100, 		0, 			0};
-	set_wp_with_index(t,1);}
+	set_command_with_index(t,1);}
 
 	if (!strcmp_P(argv[1].str, PSTR("wp"))) {
 
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_WAYPOINT,  			WP_OPTION_RELATIVE,		15, 0, 0, 0};
-		set_wp_with_index(t,2);}
+		set_command_with_index(t,2);}
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH,   	WP_OPTION_YAW,      0, 		0, 		0,		0};
-		set_wp_with_index(t,3);}
+		set_command_with_index(t,3);}
 
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_LAND,				0,      0, 		0, 		0,		0};
-		set_wp_with_index(t,4);}
+		set_command_with_index(t,4);}
 
 	} else {
 		//2250 = 25 meteres
 		// CMD										opt		p1		//alt		//NS		//WE
 		{Location t = {MAV_CMD_NAV_LOITER_TIME,   	0,      0,  	100, 		1100, 		0};
-		set_wp_with_index(t,2);}
+		set_command_with_index(t,2);}
 
 		// CMD										opt		dir		angle/deg	deg/s	relative
 		{Location t = {MAV_CMD_CONDITION_YAW,		0,      1, 		360, 		60, 	1};
-		set_wp_with_index(t,3);}
+		set_command_with_index(t,3);}
 
 		// CMD										opt
 		{Location t = {MAV_CMD_NAV_LAND,			0,      0, 		0, 			0, 		0};
-		set_wp_with_index(t,4);}
+		set_command_with_index(t,4);}
 
 	}