From 29043d62e9be8214f4232b6310699ff868ac7934 Mon Sep 17 00:00:00 2001 From: Stavros Korokithakis Date: Fri, 12 Mar 2021 19:21:49 +0200 Subject: [PATCH] Add FLIGHT_OPTION to climb before RTL --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Parameters.cpp | 2 +- ArduPlane/defines.h | 1 + ArduPlane/mode_rtl.cpp | 37 ++++++++----- .../ArduPlane_Tests/ClimbBeforeTurn/flaps.txt | 13 +++++ Tools/autotest/arduplane.py | 55 +++++++++++++++++++ 6 files changed, 95 insertions(+), 15 deletions(-) create mode 100644 Tools/autotest/ArduPlane_Tests/ClimbBeforeTurn/flaps.txt diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 05527b8b5c..5f0acc6ad5 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -503,7 +503,7 @@ void Plane::update_alt() float target_alt = relative_target_altitude_cm(); - if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) { + if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) { // ensure we do the initial climb in RTL. We add an extra // 10m in the demanded height to push TECS to climb // quickly diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 52f4015be7..2a30f8a15f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1143,7 +1143,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options // @Description: Flight mode specific options - // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB + // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL. // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 9de96adc9d..5dc12e94d9 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -152,6 +152,7 @@ enum FlightOptions { CRUISE_TRIM_THROTTLE = (1 << 1), DISABLE_TOFF_ATTITUDE_CHK = (1 << 2), CRUISE_TRIM_AIRSPEED = (1 << 3), + CLIMB_BEFORE_TURN = (1 << 4), }; enum CrowFlapOptions { diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 02fcdecfe5..ab3acc667e 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -19,20 +19,31 @@ void ModeRTL::update() plane.calc_nav_pitch(); plane.calc_throttle(); - if (plane.g2.rtl_climb_min > 0) { + bool alt_threshold_reached = false; + if (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN) { + // Climb to ALT_HOLD_RTL before turning. This overrides RTL_CLIMB_MIN. + alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt; + } else 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.prev_WP_loc = plane.current_loc; - plane.setup_glide_slope(); - 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); - } + when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT + until we have climbed by RTL_CLIMB_MIN meters + */ + alt_threshold_reached = (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min; + } else { + return; + } + + if (!plane.rtl.done_climb && alt_threshold_reached) { + plane.prev_WP_loc = plane.current_loc; + plane.setup_glide_slope(); + plane.rtl.done_climb = true; + } + if (!plane.rtl.done_climb) { + // Constrain the roll limit as a failsafe, that way if something goes wrong the plane will + // eventually turn back and go to RTL instead of going perfectly straight. This also leaves + // some leeway for fighting wind. + 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); } } diff --git a/Tools/autotest/ArduPlane_Tests/ClimbBeforeTurn/flaps.txt b/Tools/autotest/ArduPlane_Tests/ClimbBeforeTurn/flaps.txt new file mode 100644 index 0000000000..93d0726c86 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/ClimbBeforeTurn/flaps.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index f311250b25..a1f9c986e3 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1618,6 +1618,57 @@ class AutoTestPlane(AutoTest): def deadreckoning_no_airspeed_sensor(self): self.deadreckoning_main(disable_airspeed_sensor=True) + def climb_before_turn(self): + self.wait_ready_to_arm() + self.set_parameter("FLIGHT_OPTIONS", 0) + self.set_parameter("ALT_HOLD_RTL", 8000) + takeoff_alt = 40 + self.takeoff(alt=takeoff_alt) + self.change_mode("CRUISE") + self.wait_distance_to_home(500, 1000, timeout=60) + self.change_mode("RTL") + expected_alt = self.get_parameter("ALT_HOLD_RTL") / 100.0 + + home = self.home_position_as_mav_location() + distance = self.get_distance(home, self.mav.location()) + + self.wait_altitude(expected_alt - 10, expected_alt + 10, relative=True) + + new_distance = self.get_distance(home, self.mav.location()) + # We should be closer to home. + if new_distance > distance: + raise NotAchievedException( + "Expected to be closer to home (was %fm, now %fm)." + % (distance, new_distance) + ) + + self.fly_home_land_and_disarm() + self.change_mode("MANUAL") + self.set_rc(3, 1000) + + self.wait_ready_to_arm() + self.set_parameter("FLIGHT_OPTIONS", 16) + self.set_parameter("ALT_HOLD_RTL", 10000) + self.takeoff(alt=takeoff_alt) + self.change_mode("CRUISE") + self.wait_distance_to_home(500, 1000, timeout=60) + self.change_mode("RTL") + + home = self.home_position_as_mav_location() + distance = self.get_distance(home, self.mav.location()) + + self.wait_altitude(expected_alt - 10, expected_alt + 10, relative=True) + + new_distance = self.get_distance(home, self.mav.location()) + # We should be farther from to home. + if new_distance < distance: + raise NotAchievedException( + "Expected to be farther from home (was %fm, now %fm)." + % (distance, new_distance) + ) + + self.fly_home_land_and_disarm() + def rtl_climb_min(self): self.wait_ready_to_arm() rtl_climb_min = 100 @@ -2828,6 +2879,10 @@ class AutoTestPlane(AutoTest): "Test RTL_CLIMB_MIN", self.rtl_climb_min), + ("ClimbBeforeTurn", + "Test climb-before-turn", + self.climb_before_turn), + ("IMUTempCal", "Test IMU temperature calibration", self.test_imu_tempcal),