diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index d7e9ce933c..2afa10bd34 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -326,6 +326,13 @@ static void clear_new_altitude()
 
 static void set_new_altitude(int32_t _new_alt)
 {
+	if(_new_alt == current_loc.alt){
+		next_WP.alt 	= _new_alt;
+		target_altitude = _new_alt;
+		alt_change_flag = REACHED_ALT;
+		return;
+	}
+
 	// just to be clear
 	next_WP.alt = current_loc.alt;