Rover: MIS_DONE_BEHAVE parameter to choose mode after mission done

APMrover2/Parameters.cpp

@@ -663,6 +663,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
     AP_SUBGROUPINFO(airspeed, "ARSPD", 37, ParametersG2, AP_Airspeed),
 
+    // @Param: MIS_DONE_BEHAVE
+    // @DisplayName: Mission done behave
+    // @Description: Mode to become after mission done
+    // @Values: 0:Hold,1:Loiter
+    // @User: Standard
+    AP_GROUPINFO("MIS_DONE_BEHAVE", 38, ParametersG2, mis_done_behave, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -384,6 +384,9 @@ public:
 
     // Airspeed
     AP_Airspeed airspeed;
+
+    // mission behave
+    AP_Int8 mis_done_behave;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -437,6 +437,11 @@ private:
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_set_reverse(const AP_Mission::Mission_Command& cmd);
 
+    enum Mis_Done_Behave {
+        MIS_DONE_BEHAVE_HOLD      = 0,
+        MIS_DONE_BEHAVE_LOITER    = 1
+    };
+
     // commands.cpp
     void update_home_from_EKF();
     bool set_home_to_current_location(bool lock);

APMrover2/commands_logic.cpp

@@ -104,7 +104,14 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 // exit_mission - callback function called from ap-mission when the mission has completed
 void Rover::exit_mission()
 {
+    // play a tone
+    AP_Notify::events.mission_complete = 1;
+    // send message
     gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete");
+
+    if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && set_mode(mode_loiter, MODE_REASON_MISSION_END)) {
+        return;
+    }
     set_mode(mode_hold, MODE_REASON_MISSION_END);
 }