Copter: add throw_nextmode

vehicle switches to mode specified in THROW_NEXTMODE parameter after the
throw is completed.

ArduCopter/Copter.h

@@ -277,7 +277,8 @@ private:
     // throw mode state
     struct {
         ThrowModeStage stage;
-    } throw_state = {Throw_Disarmed};
+        bool nextmode_attempted;
+    } throw_state = {Throw_Disarmed, false};
 
     uint32_t precland_last_update_ms;
 
@@ -852,6 +853,7 @@ private:
     bool throw_detected();
     bool throw_attitude_good();
     bool throw_height_good();
+    bool throw_position_good();
 
     bool rtl_init(bool ignore_checks);
     void rtl_restart_without_terrain();

ArduCopter/Parameters.cpp

@@ -974,6 +974,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Button/AP_Button.cpp
     AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button),
 
+    // @Param: THROW_NEXTMODE
+    // @DisplayName: Throw mode's follow up mode
+    // @Description: Vehicle will switch to this mode after the throw is successfully completed.  Default is to stay in throw mode (18)
+    // @Values: 3:Auto,4:Guided,6:RTL,9:Land,17:Brake,18:Throw
+    // @User: Standard
+    AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18),
+
     AP_GROUPEND
 };
 

ArduCopter/Parameters.h

@@ -550,6 +550,9 @@ public:
 
     // button checking
     AP_Button button;
+
+    // Throw mode state
+    AP_Int8 throw_nextmode;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/control_throw.cpp

@@ -16,7 +16,9 @@ bool Copter::throw_init(bool ignore_checks)
         return false;
     }
 
-    // this mode needs a position reference
+    // init state
+    throw_state.nextmode_attempted = false;
+
     return true;
 }
 
@@ -113,6 +115,22 @@ void Copter::throw_run()
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
         set_auto_armed(true);
+    } else if (throw_state.stage == Throw_PosHold && throw_position_good()) {
+        if (!throw_state.nextmode_attempted) {
+            switch (g2.throw_nextmode) {
+                case AUTO:
+                case GUIDED:
+                case RTL:
+                case LAND:
+                case BRAKE:
+                    set_mode((control_mode_t)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);
+                    break;
+                default:
+                    // do nothing
+                    break;
+            }
+            throw_state.nextmode_attempted = true;
+        }
     }
 
     // Throw State Processing
@@ -225,3 +243,8 @@ bool Copter::throw_height_good()
     return (pos_control.get_alt_error() < 50.0f);
 }
 
+bool Copter::throw_position_good()
+{
+    // check that our horizontal position error is within 50cm
+    return (pos_control.get_horizontal_error() < 50.0f);
+}

ArduCopter/defines.h

@@ -126,6 +126,7 @@ enum mode_reason_t {
     MODE_REASON_FLIP_COMPLETE,
     MODE_REASON_AVOIDANCE,
     MODE_REASON_AVOIDANCE_RECOVERY,
+    MODE_REASON_THROW_COMPLETE,
 };
 
 // Tuning enumeration