Copter: add rtl to control_auto

ArduCopter/control_auto.pde

@@ -5,25 +5,16 @@
  *
  * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
  * Command execution code (i.e. command_logic.pde) should:
- *      a) switch to Auto mode with set_mode() function.  This will cause auto_init to be called
- *      b) call one of the three auto initialisation functions: auto_wp(), auto_takeoff, auto_land
+ *      a) switch to Auto flight mode with set_mode() function.  This will cause auto_init to be called
+ *      b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
  *      c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
  * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
  *      correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
  */
 
-enum AutoMode {
-    Auto_TakeOff,
-    Auto_WP,
-    Auto_Land,
-    Auto_Circle
-};
-
-static AutoMode auto_mode;   // controls which auto controller is run
-
 /*
  *  While in the auto flight mode, navigation or do/now commands can be run.
- *  Code in this file implements the navigation commands which
+ *  Code in this file implements the navigation commands
  */
 
 // auto_init - initialise auto controller
@@ -58,6 +49,10 @@ static void auto_run()
         auto_land_run();
         break;
 
+    case Auto_RTL:
+        auto_rtl_run();
+        break;
+
     case Auto_Circle:
         auto_circle_run();
         break;
@@ -235,9 +230,28 @@ static void auto_land_run()
     control_yaw = angle_target.z;
 }
 
+// auto_rtl_start - initialises RTL in AUTO flight mode
+static void auto_rtl_start()
+{
+    auto_mode = Auto_RTL;
+
+    // call regular rtl flight mode initialisation and ask it to ignore checks
+    rtl_init(true);
+}
+
+// auto_rtl_run - rtl in AUTO flight mode
+//      called by auto_run at 100hz or more
+void auto_rtl_run()
+{
+    // call regular rtl flight mode run function
+    rtl_run();
+}
+
 // auto_circle_start - initialises controller to fly a circle in AUTO flight mode
 static void auto_circle_start(const Vector3f& center)
 {
+    auto_mode = Auto_Circle;
+
     // set circle center
     circle_nav.set_center(center);
 }