diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index d27beca79e9ca2..b1cd4a5652f8db 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -498,14 +498,18 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
 ///     cmd.index is updated with it's new position in the mission
 bool AP_Mission::add_cmd(Mission_Command& cmd)
 {
+    // Home is always the index 0.
+    // Commands should be added after home.
+    const uint16_t index = MAX(_cmd_total, AP_MISSION_FIRST_REAL_COMMAND);
+
     // attempt to write the command to storage
-    bool ret = write_cmd_to_storage(_cmd_total, cmd);
+    bool ret = write_cmd_to_storage(index, cmd);
 
     if (ret) {
         // update command's index
-        cmd.index = _cmd_total;
+        cmd.index = index;
         // increment total number of commands
-        _cmd_total.set_and_save(_cmd_total + 1);
+        _cmd_total.set_and_save(index + 1);
     }
 
     return ret;
@@ -516,8 +520,8 @@ bool AP_Mission::add_cmd(Mission_Command& cmd)
 ///     returns true if successfully replaced, false on failure
 bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd)
 {
-    // sanity check index
-    if (index >= (unsigned)_cmd_total) {
+    // sanity check index, can't replace home or a waypoint which does not exist
+    if ((index == 0) || index >= (unsigned)_cmd_total) {
         return false;
     }
 
@@ -704,11 +708,19 @@ bool AP_Mission::restart_current_nav_cmd()
 // returns false on any issue at all.
 bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet)
 {
+    // Cannot set home
+    if (index < AP_MISSION_FIRST_REAL_COMMAND) {
+        return false;
+    }
+
     // this is the on-storage format
     AP_Mission::Mission_Command cmd {};
 
+    // Missions start at 1
+    const uint16_t mission_index = MAX(num_commands(), AP_MISSION_FIRST_REAL_COMMAND);
+
     // can't handle request for anything bigger than the mission size+1...
-    if (index > num_commands()) {
+    if (index > mission_index) {
         return false;
     }
 
@@ -719,7 +731,7 @@ bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet
 
     // A request to set the 'next' item after the end is how we add an extra
     //  item to the list, thus allowing us to write entire missions if needed.
-    if (index == num_commands()) {
+    if (index == mission_index) {
         return add_cmd(cmd);
     }
 
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index f3a1c8a043fa92..c60ad7d32cfc53 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -631,11 +631,6 @@ class AP_Mission
     ///     true is return if successful
     bool read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const;
 
-    /// write_cmd_to_storage - write a command to storage
-    ///     cmd.index is used to calculate the storage location
-    ///     true is returned if successful
-    bool write_cmd_to_storage(uint16_t index, const Mission_Command& cmd);
-
     /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage
     ///     home is taken directly from ahrs
     void write_home_to_storage();
@@ -948,6 +943,11 @@ class AP_Mission
     bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd);
     bool start_command_fence(const AP_Mission::Mission_Command& cmd);
 
+    /// write_cmd_to_storage - write a command to storage
+    ///     cmd.index is used to calculate the storage location
+    ///     true is returned if successful
+    bool write_cmd_to_storage(uint16_t index, const Mission_Command& cmd);
+
     /*
       handle format conversion of storage format to allow us to update
       format to take advantage of new packing