Copter: refactor RTL to compute full path on initialization

ArduCopter/Copter.cpp

@@ -49,7 +49,6 @@ Copter::Copter(void) :
     guided_mode(Guided_TakeOff),
     rtl_state(RTL_InitialClimb),
     rtl_state_complete(false),
-    rtl_alt(0.0f),
     circle_pilot_yaw_override(false),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),

ArduCopter/Copter.h

@@ -340,7 +340,15 @@ private:
     // RTL
     RTLState rtl_state;  // records state of rtl (initial climb, returning home, etc)
     bool rtl_state_complete; // set to true if the current state is completed
-    float rtl_alt;     // altitude the vehicle is returning at
+
+    struct {
+        // NEU w/ origin-relative altitude
+        Vector3f origin_point;
+        Vector3f climb_target;
+        Vector3f return_target;
+        Vector3f descent_target;
+        bool land;
+    } rtl_path;
 
     // Circle
     bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@@ -802,7 +810,8 @@ private:
     void rtl_descent_run();
     void rtl_land_start();
     void rtl_land_run();
-    float get_RTL_alt();
+    void rtl_build_path();
+    float rtl_compute_return_alt_above_origin(float rtl_return_dist);
     bool sport_init(bool ignore_checks);
     void sport_run();
     bool stabilize_init(bool ignore_checks);

ArduCopter/control_rtl.cpp

@@ -13,6 +13,7 @@
 bool Copter::rtl_init(bool ignore_checks)
 {
     if (position_ok() || ignore_checks) {
+        rtl_build_path();
         rtl_climb_start();
         return true;
     }else{
@@ -34,10 +35,10 @@ void Copter::rtl_run()
             rtl_loiterathome_start();
             break;
         case RTL_LoiterAtHome:
-            if (g.rtl_alt_final > 0 && !failsafe.radio) {
-                rtl_descent_start();
-            }else{
+            if (rtl_path.land || failsafe.radio) {
                 rtl_land_start();
+            }else{
+                rtl_descent_start();
             }
             break;
         case RTL_FinalDescent:
@@ -79,7 +80,6 @@ void Copter::rtl_climb_start()
 {
     rtl_state = RTL_InitialClimb;
     rtl_state_complete = false;
-    rtl_alt = get_RTL_alt();
 
     // initialise waypoint and spline controller
     wp_nav.wp_and_spline_init();
@@ -89,22 +89,8 @@ void Copter::rtl_climb_start()
         wp_nav.set_speed_xy(g.rtl_speed_cms);
     }
 
-    // get horizontal stopping point
-    Vector3f destination;
-    wp_nav.get_wp_stopping_point_xy(destination);
-
-#if AC_RALLY == ENABLED
-    // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
-    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
-    rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
-    rally_point.alt = MAX(rally_point.alt, current_loc.alt);    // ensure we do not descend before reaching home
-    destination.z = pv_alt_above_origin(rally_point.alt);
-#else
-    destination.z = pv_alt_above_origin(rtl_alt);
-#endif
-
     // set the destination
-    wp_nav.set_wp_destination(destination);
+    wp_nav.set_wp_destination(rtl_path.climb_target);
     wp_nav.set_fast_waypoint(true);
 
     // hold current yaw during initial climb
@@ -117,19 +103,7 @@ void Copter::rtl_return_start()
     rtl_state = RTL_ReturnHome;
     rtl_state_complete = false;
 
-    // set target to above home/rally point
-#if AC_RALLY == ENABLED
-    // rally_point will be the nearest rally point or home.  uses absolute altitudes
-    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
-    rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
-    rally_point.alt = MAX(rally_point.alt, current_loc.alt);    // ensure we do not descend before reaching home
-    Vector3f destination = pv_location_to_vector(rally_point);
-#else
-    Vector3f destination = pv_location_to_vector(ahrs.get_home());
-    destination.z = pv_alt_above_origin(rtl_alt);
-#endif
-
-    wp_nav.set_wp_destination(destination);
+    wp_nav.set_wp_destination(rtl_path.return_target);
 
     // initialise yaw to point home (maybe)
     set_auto_yaw_mode(get_default_auto_yaw_mode(true));
@@ -313,14 +287,14 @@ void Copter::rtl_descent_run()
     wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     // call z-axis position controller
-    pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);
+    pos_control.set_alt_target_with_slew(rtl_path.descent_target.z, G_Dt);
     pos_control.update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
 
     // check if we've reached within 20cm of final altitude
-    rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
+    rtl_state_complete = fabsf(rtl_path.descent_target.z - inertial_nav.get_altitude()) < 20.0f;
 }
 
 // rtl_loiterathome_start - initialise controllers to loiter over home
@@ -415,13 +389,46 @@ void Copter::rtl_land_run()
     rtl_state_complete = ap.land_complete;
 }
 
-// get_RTL_alt - return altitude which vehicle should return home at
-//      altitude is in cm above home
-float Copter::get_RTL_alt()
+void Copter::rtl_build_path()
+{
+    // origin point is our stopping point
+    pos_control.get_stopping_point_xy(rtl_path.origin_point);
+    pos_control.get_stopping_point_z(rtl_path.origin_point);
+
+    // set return target to nearest rally point or home position
+#if AC_RALLY == ENABLED
+    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
+    rtl_path.return_target = pv_location_to_vector(rally_point);
+#else
+    rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
+#endif
+
+    Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
+
+    float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
+
+    // compute return altitude
+    rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
+
+    // climb target is above our origin point at the return altitude
+    rtl_path.climb_target.x = rtl_path.origin_point.x;
+    rtl_path.climb_target.y = rtl_path.origin_point.y;
+    rtl_path.climb_target.z = rtl_path.return_target.z;
+
+    // descent target is below return target at rtl_alt_final
+    rtl_path.descent_target.x = rtl_path.return_target.x;
+    rtl_path.descent_target.y = rtl_path.return_target.y;
+    rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
+
+    // set land flag
+    rtl_path.land = g.rtl_alt_final <= 0;
+}
+
+// return altitude in cm above origin at which vehicle should return home
+float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
 {
     // maximum of current altitude + climb_min and rtl altitude
-    float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);
-    ret = MAX(ret, RTL_ALT_MIN);
+    float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
 
 #if AC_FENCE == ENABLED
     // ensure not above fence altitude if alt fence is enabled
@@ -430,6 +437,13 @@ float Copter::get_RTL_alt()
     }
 #endif
 
-    return ret;
-}
+#if AC_RALLY == ENABLED
+    // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
+    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt);
+    rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
+    rally_point.alt = MAX(rally_point.alt, current_loc.alt);    // ensure we do not descend before reaching home
+    ret = rally_point.alt;
+#endif
 
+    return pv_alt_above_origin(ret);
+}