AP_Mission: only continue after land if next waypoint is a takeoff

ArduCopter/mode_auto.cpp

@@ -1658,7 +1658,7 @@ bool ModeAuto::verify_land()
         case State::Descending:
             // rely on THROTTLE_LAND mode to correctly update landing status
             retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
-            if (retval && !mission.continue_after_land() && copter.motors->armed()) {
+            if (retval && !mission.continue_after_land_check_for_takeoff() && copter.motors->armed()) {
                 /*
                   we want to stop mission processing on land
                   completion. Disarm now, then return false. This

libraries/AP_Mission/AP_Mission.cpp

@@ -149,15 +149,10 @@ void AP_Mission::resume()
     //      update will take care of finding and starting the nav command
 }
 
-/// check mission starts with a takeoff command
-bool AP_Mission::starts_with_takeoff_cmd()
+/// check if the next nav command is a takeoff, skipping delays
+bool AP_Mission::is_takeoff_next(uint16_t cmd_index)
 {
     Mission_Command cmd = {};
-    uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
-    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
-        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
-    }
-
     // check a maximum of 16 items, remembering that missions can have
     // loops in them
     for (uint8_t i=0; i<16; i++, cmd_index++) {
@@ -180,6 +175,33 @@ bool AP_Mission::starts_with_takeoff_cmd()
     return false;
 }
 
+/// check mission starts with a takeoff command
+bool AP_Mission::starts_with_takeoff_cmd()
+{
+    uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
+    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
+        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
+    }
+    return is_takeoff_next(cmd_index);
+}
+
+/*
+    return true if MIS_OPTIONS is set to allow continue of mission
+    logic after a land and the next waypoint is a takeoff. If this
+    is false then after a landing is complete the vehicle should 
+    disarm and mission logic should stop
+*/
+bool AP_Mission::continue_after_land_check_for_takeoff()
+{
+    if (!continue_after_land()) {
+        return false;
+    }
+    if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
+        return false;
+    }
+    return is_takeoff_next(_nav_cmd.index+1);
+}
+
 /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
 void AP_Mission::start_or_resume()
 {

libraries/AP_Mission/AP_Mission.h

@@ -577,9 +577,11 @@ class AP_Mission
 
     /*
       return true if MIS_OPTIONS is set to allow continue of mission
-      logic after a land. If this is false then after a landing is
-      complete the vehicle should disarm and mission logic should stop
+      logic after a land and the next waypoint is a takeoff. If this
+      is false then after a landing is complete the vehicle should 
+      disarm and mission logic should stop
      */
+    bool continue_after_land_check_for_takeoff(void);
     bool continue_after_land(void) const {
         return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0;
     }
@@ -676,6 +678,9 @@ class AP_Mission
     /// sanity checks that the masked fields are not NaN's or infinite
     static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);
 
+    /// check if the next nav command is a takeoff, skipping delays
+    bool is_takeoff_next(uint16_t start_index);
+
     // pointer to main program functions
     mission_cmd_fn_t        _cmd_start_fn;  // pointer to function which will be called when a new command is started
     mission_cmd_fn_t        _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing