diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 7da24f55f8..05f1e1e2a1 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -260,6 +260,7 @@ byte	command_must_index;					// current command memory location
 byte	command_may_index;					// current command memory location
 byte	command_must_ID;					// current command ID
 byte	command_may_ID;						// current command ID
+byte 	wp_verify_byte;						// used for tracking state of navigating waypoints
 
 float cos_roll_x 	= 1;
 float cos_pitch_x 	= 1;
diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt
index 622efaded9..09dfef520c 100644
--- a/ArduCopterMega/command description.txt	
+++ b/ArduCopterMega/command description.txt	
@@ -22,10 +22,10 @@ Command Structure in bytes
 Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
 ***********************************
 Command ID	Name								Parameter 1			Altitude			Latitude			Longitude
-0x10 / 16	MAV_CMD_NAV_WAYPOINT				-					altitude			lat					lon
+0x10 / 16	MAV_CMD_NAV_WAYPOINT				delay (seconds)		altitude			lat					lon
 0x11 / 17	MAV_CMD_NAV_LOITER_UNLIM 			(forever)			altitude			lat					lon
 0x12 / 18	MAV_CMD_NAV_LOITER_TURNS			turns				altitude			lat					lon
-0x13 / 19	MAV_CMD_NAV_LOITER_TIME				time (seconds*10)	altitude			lat					lon
+0x13 / 19	MAV_CMD_NAV_LOITER_TIME				-					time (seconds)		-					-
 0x14 / 20	MAV_CMD_NAV_RETURN_TO_LAUNCH		-					altitude			lat					lon
 0x15 / 21	MAV_CMD_NAV_LAND					-					altitude			lat					lon
 0x16 / 22	MAV_CMD_NAV_TAKEOFF					takeoff pitch		altitude			-					-
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index fafa611e34..925b9c4d61 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/ArduCopterMega/commands_logic.pde
@@ -231,6 +231,14 @@ 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)
+
+	if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){
+		// we don't need to worry about it
+		wp_verify_byte |= NAV_ALTITUDE;
+	}
 }
 
 void do_land()
@@ -327,31 +335,39 @@ bool verify_land()
 
 bool verify_nav_wp()
 {
-	bool alt = true;
-
 	update_crosstrack();
 
-	if (next_WP.options & WP_OPTION_ALT_REQUIRED){
-		alt = (current_loc.alt > next_WP.alt);
+	// Altitude checking
+	if(next_WP.options & WP_OPTION_ALT_REQUIRED){
+		// we desire a certain minimum altitude
+		if (current_loc.alt > next_WP.alt){
+			// we have reached that altitude
+			wp_verify_byte |= NAV_ALTITUDE;
+		}
 	}
 
-	if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-		//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
-		//SendDebugln(command_must_index,DEC);
+	// Distance checking
+	if((wp_distance > 0) && (wp_distance <= g.waypoint_radius)){
+		wp_verify_byte |= NAV_LOCATION;
+		if(loiter_time == 0){
+			loiter_time = millis();
+		}
+	}
 
-		if (alt == true){
-			char message[30];
-			sprintf(message,"Reached Waypoint #%i",command_must_index);
-			gcs.send_text(SEVERITY_LOW,message);
-			return true;
-		}else{
-			return false;
+	// Hold at Waypoint checking
+	if(wp_verify_byte & NAV_LOCATION){							// we have reached our goal
+
+		if ((millis() - loiter_time) > loiter_time_max) {
+			wp_verify_byte |= NAV_DELAY;
+			//gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
+			//Serial.println("vlt done");
 		}
 	}
 
-	// Have we passed the WP?
-	if(alt && (loiter_sum > 90)){
-		gcs.send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
+	if(wp_verify_byte == 7){
+		char message[30];
+		sprintf(message,"Reached Waypoint #%i",command_must_index);
+		gcs.send_text(SEVERITY_LOW,message);
 		return true;
 	}else{
 		return false;
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index a474642256..85e5eaca5b 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -102,6 +102,7 @@
 #define YAW_RATE 2
 
 // CH_6 Tuning
+// -----------
 #define CH6_NONE 0
 #define CH6_STABLIZE_KP 1
 #define CH6_STABLIZE_KD 2
@@ -111,6 +112,11 @@
 #define CH6_SONAR_KD 6
 #define CH6_Y6_SCALING 7
 
+// nav byte mask
+// -------------
+#define NAV_LOCATION 1
+#define NAV_ALTITUDE 2
+#define NAV_DELAY    4
 
 
 // Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library