From 418f137f52bcab82829c8240ac4bfab6fda44dbb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 14 Jan 2025 16:44:10 +0000 Subject: [PATCH] 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 | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index bcd61da9ab..13bb7bb167 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/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()) {