diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index 6fc37e2f1b..7c4418ba6c 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -2,7 +2,7 @@
 
 static void init_commands()
 {
-	// zero is home, but we always load the next command (1), in the code.
+	// Zero is home, the curren command
     g.command_index = 0;
 
     // This are registers for the current may and must commands
@@ -28,7 +28,7 @@ static struct Location get_cmd_with_index(int i)
 
 	// Find out proper location in memory by using the start_byte position + the index
 	// --------------------------------------------------------------------------------
-	if (i > g.command_total) {
+	if (i >= g.command_total) {
 		// we do not have a valid command to load
 		// return a WP with a "Blank" id
 		temp.id = CMD_BLANK;
@@ -77,7 +77,13 @@ static struct Location get_cmd_with_index(int i)
 // -------
 static void set_command_with_index(struct Location temp, int i)
 {
-	i = constrain(i, 0, g.command_total.get());
+	Serial.printf("set_command: %d with id: %d\n", i, temp.id);
+	i = constrain(i, 0, (g.command_total.get() -1));
+
+	// store home as 0 altitude!!!
+	if (i == 0)
+		temp.alt = 0;
+
 	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
 
 	eeprom_write_byte((uint8_t *)	mem, temp.id);
@@ -100,7 +106,7 @@ static void set_command_with_index(struct Location temp, int i)
 
 static void increment_WP_index()
 {
-    if (g.command_index < g.command_total) {
+    if (g.command_index < (g.command_total-1)) {
         g.command_index++;
 	}
 
@@ -129,18 +135,6 @@ static int32_t read_alt_to_hold()
 // It's not currently used
 //********************************************************************************
 
-/*static Location get_LOITER_home_wp()
-{
-	//so we know where we are navigating from
-	next_WP = current_loc;
-
-	// read home position
-	struct Location temp 	= get_cmd_with_index(0);	// 0 = home
-	temp.id 				= MAV_CMD_NAV_LOITER_UNLIM;
-	temp.alt 				= read_alt_to_hold();
-	return temp;
-}
-*/
 /*
 This function sets the next waypoint command
 It precalculates all the necessary stuff.