AP_Mission: make sure home is set in index 0 and and not overwritten when adding or replacing commands

libraries/AP_Mission/AP_Mission.cpp

@@ -498,6 +498,11 @@ 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)
 {
+    // Add home if its not already present
+    if (_cmd_total < 1) {
+        write_home_to_storage();
+    }
+
     // attempt to write the command to storage
     bool ret = write_cmd_to_storage(_cmd_total, cmd);
 
@@ -521,6 +526,12 @@ bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd)
         return false;
     }
 
+    // Writing index zero is not allowed, it must be home
+    if (index == 0) {
+        // Really should be returning false in this case, but in order to not break things we return true
+        return true;
+    }
+
     // attempt to write the command to storage
     return write_cmd_to_storage(index, cmd);
 }
@@ -717,6 +728,16 @@ bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet
         return false;
     }
 
+    // Writing index zero is not allowed, it must be home
+    if (index == 0) {
+        // If home is not already loaded add it so cmd_total is incremented to 1 as would be expected when the returning true
+        if (_cmd_total < 1) {
+            write_home_to_storage();
+        }
+        // Really should be returning false in this case, but in order to not break things we return true
+        return true;
+    }
+
     // 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()) {