diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 14e1e2b0b0..75c6663949 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -217,12 +217,26 @@ bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) cons
     // read WP position
     pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
 
+    cmd.content.location.id = hal.storage->read_byte(pos_in_storage);
+    pos_in_storage++;
+
+    cmd.content.location.options = hal.storage->read_byte(pos_in_storage);
+    pos_in_storage++;
+
+    cmd.content.location.p1 = hal.storage->read_byte(pos_in_storage);
+    pos_in_storage++;
+
+    cmd.content.location.alt = hal.storage->read_dword(pos_in_storage);
+    pos_in_storage += 4;
+
+    cmd.content.location.lat = hal.storage->read_dword(pos_in_storage);
+    pos_in_storage += 4;
+
+    cmd.content.location.lng = hal.storage->read_dword(pos_in_storage);
+
     // set command's index to it's position in eeprom
     cmd.index = index;
 
-    // read command from eeprom
-    hal.storage->read_block(cmd.content.bytes, pos_in_storage, AP_MISSION_EEPROM_COMMAND_SIZE);
-
     // set command from location's command
     // To-Do: remove id (and p1?) from Location structure
     cmd.id = cmd.content.location.id;
@@ -244,12 +258,23 @@ bool AP_Mission::write_cmd_to_storage(uint8_t index, Mission_Command& cmd)
     // calculate where in storage the command should be placed
     uint16_t pos_in_storage = AP_MISSION_EEPROM_START_BYTE + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
 
-    // set location id to cmd.id
-    // To-Do: remove id (and p1?) from Location structure
     cmd.content.location.id = cmd.id;
+    hal.storage->write_byte(pos_in_storage, cmd.content.location.id);
 
-    // write block to eeprom
-    hal.storage->write_block(pos_in_storage, cmd.content.bytes, AP_MISSION_EEPROM_COMMAND_SIZE);
+    pos_in_storage++;
+    hal.storage->write_byte(pos_in_storage, cmd.content.location.options);
+
+    pos_in_storage++;
+    hal.storage->write_byte(pos_in_storage, cmd.content.location.p1);
+
+    pos_in_storage++;
+    hal.storage->write_dword(pos_in_storage, cmd.content.location.alt);
+
+    pos_in_storage += 4;
+    hal.storage->write_dword(pos_in_storage, cmd.content.location.lat);
+
+    pos_in_storage += 4;
+    hal.storage->write_dword(pos_in_storage, cmd.content.location.lng);
 
     // return success
     return true;
@@ -295,7 +320,7 @@ bool AP_Mission::advance_current_nav_cmd()
     // get starting point for search
     cmd_index = _nav_cmd.index;
     if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
-        cmd_index = 0;
+        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
     }else{
         // start from one position past the current nav command
         cmd_index++;
@@ -303,7 +328,6 @@ bool AP_Mission::advance_current_nav_cmd()
 
     // search until we find next nav command or reach end of command list
     while (!_flags.nav_cmd_loaded) {
-
         // get next command
         if (!get_next_cmd(cmd_index, cmd, true)) {
             return false;
@@ -348,7 +372,7 @@ void AP_Mission::advance_current_do_cmd()
     // get starting point for search
     cmd_index = _do_cmd.index;
     if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
-        cmd_index = 0;
+        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
     }else{
         // start from one position past the current do command
         cmd_index++;
@@ -383,7 +407,6 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
 
     // search until the end of the mission command list
     while(cmd_index < _cmd_total) {
-
         // load the next command
         read_cmd_from_storage(cmd_index, temp_cmd);
 
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index 2307cd920b..2162054748 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -37,6 +37,8 @@
 #define AP_MISSION_CMD_INDEX_NONE           255     // command index of 255 means invalid or missing command
 #define AP_MISSION_JUMP_TIMES_MAX           255     // maximum number of times a jump can be executed.  Used when jump tracking fails (i.e. when too many jumps in mission)
 
+#define AP_MISSION_FIRST_REAL_COMMAND       1       // command #0 reserved to hold home position
+
 /// @class    AP_Mission
 /// @brief    Object managing Mission
 class AP_Mission {
@@ -56,8 +58,6 @@ public:
 
         // location
         Location location;      // Waypoint location
-
-        uint8_t bytes[AP_MISSION_EEPROM_COMMAND_SIZE];
     };
 
     // command structure
@@ -154,6 +154,10 @@ public:
     /// get_active_do_cmd - returns active "do" command
     const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
 
+    // jump_to_cmd - jumps to command specified by index
+    //      To-Do: implement this function!
+    bool jump_to_cmd(uint8_t index) { return false; }
+
     /// load_cmd_from_storage - load command from storage
     ///     true is return if successful
     bool read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const;