Copter: FlightMode - convert SMARTRTL flight mode

ArduCopter/ArduCopter.cpp

@@ -535,6 +535,11 @@ void Copter::update_GPS(void)
     }
 }
 
+void Copter::smart_rtl_save_position()
+{
+    flightmode_smartrtl.save_position();
+}
+
 void Copter::init_simple_bearing()
 {
     // capture current cos_yaw and sin_yaw values

ArduCopter/Copter.cpp

@@ -33,7 +33,6 @@ Copter::Copter(void)
     home_bearing(0),
     home_distance(0),
     wp_distance(0),
-    smart_rtl_state(SmartRTL_PathFollow),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
     super_simple_last_bearing(0),

ArduCopter/Copter.h

@@ -392,9 +392,6 @@ private:
         float descend_max; // centimetres
     } nav_payload_place;
 
-    // SmartRTL
-    SmartRTLState smart_rtl_state;  // records state of SmartRTL
-
     // SIMPLE Mode
     // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
     // or in SuperSimple mode when the vehicle leaves a 20m radius from home.
@@ -770,14 +767,7 @@ private:
     void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();
 
-    bool smart_rtl_init(bool ignore_checks);
-    void smart_rtl_exit();
-    void smart_rtl_run();
-    void smart_rtl_wait_cleanup_run();
-    void smart_rtl_path_follow_run();
-    void smart_rtl_pre_land_position_run();
-    void smart_rtl_land();
-    void smart_rtl_save_position();
+    void smart_rtl_save_position(); // method for scheduler to call
 
     void crash_check();
     void parachute_check();
@@ -1020,6 +1010,8 @@ private:
 
     Copter::FlightMode_GUIDED_NOGPS flightmode_guided_nogps{*this};
 
+    Copter::FlightMode_SMARTRTL flightmode_smartrtl{*this};
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

ArduCopter/FlightMode.h

@@ -544,6 +544,13 @@ protected:
     const char *name() const override { return "RTL"; }
     const char *name4() const override { return "RTL "; }
 
+    void descent_start();
+    void descent_run();
+    void land_start();
+    void land_run(bool disarm_on_land);
+
+    void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; }
+
 private:
 
     void climb_start();
@@ -551,10 +558,6 @@ private:
     void climb_return_run();
     void loiterathome_start();
     void loiterathome_run();
-    void descent_start();
-    void descent_run();
-    void land_start();
-    void land_run(bool disarm_on_land);
     void build_path(bool terrain_following_allowed);
     void compute_return_target(bool terrain_following_allowed);
 
@@ -895,3 +898,40 @@ protected:
 private:
 
 };
+
+
+class FlightMode_SMARTRTL : public FlightMode_RTL {
+
+public:
+
+    FlightMode_SMARTRTL(Copter &copter) :
+        FlightMode_SMARTRTL::FlightMode_RTL(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return true; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override {
+        return false;
+    }
+    bool is_autopilot() const override { return true; }
+
+    void save_position();
+    void exit();
+
+protected:
+
+    const char *name() const override { return "SMARTRTL"; }
+    const char *name4() const override { return "SRTL"; }
+
+private:
+
+    void wait_cleanup_run();
+    void path_follow_run();
+    void pre_land_position_run();
+    void land();
+    SmartRTLState smart_rtl_state = SmartRTL_PathFollow;
+
+};

ArduCopter/control_smart_rtl.cpp

@@ -7,9 +7,9 @@
  * Once the copter is close to home, it will run a standard land controller.
  */
 
-bool Copter::smart_rtl_init(bool ignore_checks)
+bool Copter::FlightMode_SMARTRTL::init(bool ignore_checks)
 {
-    if ((position_ok() || ignore_checks) && g2.smart_rtl.is_active()) {
+    if ((copter.position_ok() || ignore_checks) && g2.smart_rtl.is_active()) {
         // initialise waypoint and spline controller
         wp_nav->wp_and_spline_init();
 
@@ -20,7 +20,7 @@ bool Copter::smart_rtl_init(bool ignore_checks)
         wp_nav->set_wp_destination(stopping_point);
 
         // initialise yaw to obey user parameter
-        set_auto_yaw_mode(get_default_auto_yaw_mode(true));
+        copter.set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
 
         // wait for cleanup of return path
         smart_rtl_state = SmartRTL_WaitForPathCleanup;
@@ -31,33 +31,33 @@ bool Copter::smart_rtl_init(bool ignore_checks)
 }
 
 // perform cleanup required when leaving smart_rtl
-void Copter::smart_rtl_exit()
+void Copter::FlightMode_SMARTRTL::exit()
 {
     g2.smart_rtl.cancel_request_for_thorough_cleanup();
 }
 
-void Copter::smart_rtl_run()
+void Copter::FlightMode_SMARTRTL::run()
 {
     switch (smart_rtl_state) {
         case SmartRTL_WaitForPathCleanup:
-            smart_rtl_wait_cleanup_run();
+            wait_cleanup_run();
             break;
         case SmartRTL_PathFollow:
-            smart_rtl_path_follow_run();
+            path_follow_run();
             break;
         case SmartRTL_PreLandPosition:
-            smart_rtl_pre_land_position_run();
+            pre_land_position_run();
             break;
         case SmartRTL_Descend:
-            rtl_descent_run(); // Re-using the descend method from normal rtl mode.
+            descent_run(); // Re-using the descend method from normal rtl mode.
             break;
         case SmartRTL_Land:
-            rtl_land_run(true); // Re-using the land method from normal rtl mode.
+            land_run(true); // Re-using the land method from normal rtl mode.
             break;
     }
 }
 
-void Copter::smart_rtl_wait_cleanup_run()
+void Copter::FlightMode_SMARTRTL::wait_cleanup_run()
 {
     // hover at current target position
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@@ -71,7 +71,7 @@ void Copter::smart_rtl_wait_cleanup_run()
     }
 }
 
-void Copter::smart_rtl_path_follow_run()
+void Copter::FlightMode_SMARTRTL::path_follow_run()
 {
     // if we are close to current target point, switch the next point to be our target.
     if (wp_nav->reached_wp_destination()) {
@@ -108,17 +108,17 @@ void Copter::smart_rtl_path_follow_run()
     }
 }
 
-void Copter::smart_rtl_pre_land_position_run()
+void Copter::FlightMode_SMARTRTL::pre_land_position_run()
 {
     // if we are close to 2m above start point, we are ready to land.
     if (wp_nav->reached_wp_destination()) {
         // choose descend and hold, or land based on user parameter rtl_alt_final
-        if (g.rtl_alt_final <= 0 || failsafe.radio) {
-            rtl_land_start();
+        if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
+            land_start();
             smart_rtl_state = SmartRTL_Land;
         } else {
-            rtl_path.descent_target.alt = g.rtl_alt_final;
-            rtl_descent_start();
+            set_descent_target_alt(copter.g.rtl_alt_final);
+            descent_start();
             smart_rtl_state = SmartRTL_Descend;
         }
     }
@@ -131,9 +131,9 @@ void Copter::smart_rtl_pre_land_position_run()
 }
 
 // save current position for use by the smart_rtl flight mode
-void Copter::smart_rtl_save_position()
+void Copter::FlightMode_SMARTRTL::save_position()
 {
-    const bool save_position = motors->armed() && (control_mode != SMART_RTL);
+    const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL);
 
-    g2.smart_rtl.update(position_ok(), save_position);
+    copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
 }

ArduCopter/flight_mode.cpp

@@ -170,7 +170,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case SMART_RTL:
-            success = smart_rtl_init(ignore_checks);
+            success = flightmode_guided_nogps.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_smartrtl;
+            }
             break;
 
         default:
@@ -239,10 +242,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-        case SMART_RTL:
-            smart_rtl_run();
-            break;
-
         default:
             break;
     }
@@ -278,7 +277,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
 
     // call smart_rtl cleanup
     if (old_control_mode == SMART_RTL) {
-        smart_rtl_exit();
+        flightmode_smartrtl.exit();
     }
 
 #if FRAME_CONFIG == HELI_FRAME
@@ -308,8 +307,6 @@ bool Copter::mode_requires_GPS()
         return flightmode->requires_GPS();
     }
     switch (control_mode) {
-        case SMART_RTL:
-            return true;
         default:
             return false;
     }
@@ -352,9 +349,6 @@ void Copter::notify_flight_mode()
         return;
     }
     switch (control_mode) {
-        case SMART_RTL:
-            // autopilot modes
-            AP_Notify::flags.autopilot_mode = true;
             break;
     default:
             // all other are manual flight modes