Plane: implement RTL_CLIMB_MIN parameter

this allows for an initial climb on RTL where roll is limited to LEVEL_ROLL_LIMIT
ArduPilot · May 18, 2020 · 8b21f51 · 8b21f51
1 parent 939331a
commit 8b21f51
Show file tree

Hide file tree

Showing 5 changed files with 42 additions and 2 deletions.
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -580,8 +580,17 @@ void Plane::update_alt()
         if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
-
-        SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
+
+        float target_alt = relative_target_altitude_cm();
+
+        if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
+            // ensure we do the initial climb in RTL. We add an extra
+            // 10m in the demanded height to push TECS to climb
+            // quickly
+            target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100);
+        }
+
+        SpdHgt_Controller->update_pitch_throttle(target_alt,
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  distance_beyond_land_wp,

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1278,6 +1278,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT),
 
+    // @Param: RTL_CLIMB_MIN
+    // @DisplayName: RTL minimum climb
+    // @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees.
+    // @Units: m
+    // @Range: 0 30
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
+
     AP_GROUPEND
 };
 

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -577,6 +577,9 @@ class ParametersG2 {
 #endif
 
     AP_Float        fs_ekf_thresh;
+
+    // min initial climb in RTL
+    AP_Int16        rtl_climb_min;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -726,6 +726,10 @@ class Plane : public AP_Vehicle {
         uint32_t last_trim_save;
     } auto_trim;
 
+    struct {
+        bool done_climb;
+    } rtl;
+
     // last time home was updated while disarmed
     uint32_t last_home_update_ms;
 

diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp
@@ -8,6 +8,7 @@ bool ModeRTL::_enter()
     plane.auto_navigation_mode = true;
     plane.prev_WP_loc = plane.current_loc;
     plane.do_RTL(plane.get_RTL_altitude());
+    plane.rtl.done_climb = false;
 
     return true;
 }
@@ -17,5 +18,19 @@ void ModeRTL::update()
     plane.calc_nav_roll();
     plane.calc_nav_pitch();
     plane.calc_throttle();
+
+    if (plane.g2.rtl_climb_min > 0) {
+        /*
+          when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
+          until we have climbed by RTL_CLIMB_MIN meters
+         */
+        if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) {
+            plane.rtl.done_climb = true;
+        }
+        if (!plane.rtl.done_climb) {
+            plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
+            plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
+        }
+    }
 }