diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index 46fe8bf5f9..379bd99d77 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -7,8 +7,9 @@ static void init_commands()
 
     // 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;
+	command_nav_index	= NO_COMMAND;
+	command_cond_index	= NO_COMMAND;
+	prev_nav_index 		= NO_COMMAND;
 
 	// clear the command queue
 	clear_command_queue();
@@ -17,7 +18,8 @@ static void init_commands()
 // forces the loading of a new command
 // queue is emptied after a new command is processed
 static void clear_command_queue(){
-	next_command.id 	= NO_COMMAND;
+	command_cond_queue.id 	= NO_COMMAND;
+	command_nav_queue.id 	= NO_COMMAND;
 }
 
 // Getters
@@ -111,7 +113,8 @@ static void set_command_with_index(struct Location temp, int i)
 		g.command_total.set_and_save(i+1);
 }
 
-static void increment_WP_index()
+/*
+//static void increment_WP_index()
 {
     if (g.command_index < (g.command_total-1)) {
         g.command_index++;
@@ -119,9 +122,9 @@ static void increment_WP_index()
 
     SendDebugln(g.command_index,DEC);
 }
-
+*/
 /*
-static void decrement_WP_index()
+//static void decrement_WP_index()
 {
     if (g.command_index > 0) {
         g.command_index.set_and_save(g.command_index - 1);
@@ -130,7 +133,7 @@ static void decrement_WP_index()
 
 static int32_t read_alt_to_hold()
 {
-	if(g.RTL_altitude < 0)
+	if(g.RTL_altitude <= 0)
 		return current_loc.alt;
 	else
 		return g.RTL_altitude;// + home.alt;