diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index ee851089e2087..ae0283c98ae17 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -95,6 +95,13 @@ void ModeRTL::navigate() } #endif + uint16_t radius = abs(plane.g.rtl_radius); + if (radius > 0) { + plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1; + } + + plane.update_loiter(radius); + if (!plane.auto_state.checked_for_autoland) { if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) || (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START && @@ -116,13 +123,6 @@ void ModeRTL::navigate() plane.auto_state.checked_for_autoland = true; } } - - uint16_t radius = abs(plane.g.rtl_radius); - if (radius > 0) { - plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1; - } - - plane.update_loiter(radius); } #if HAL_QUADPLANE_ENABLED diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 12b7cfda70db0..8cd4bc80826a2 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1662,6 +1662,115 @@ def DCMClimbRate(self): # Force disarm self.disarm_vehicle(force=True) + def RTL_AUTOLAND_1(self): + '''test behaviour when RTL_AUTOLAND==1''' + + self.set_parameters({ + "RTL_AUTOLAND": 1, + }) + + # when RTL is entered and RTL_AUTOLAND is 1 we should fly home + # then to the landing sequence. This mission puts the landing + # sequence well to the West of home so if we go directly there + # we won't come within 200m of home + wps = self.create_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + # fly North + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30), + # add a waypoint 1km North (which we will look for and trigger RTL + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30), + + # *exciting* landing sequence is ~1km West and points away from Home. + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_LAND_START, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5), + (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0), + ]) + self.check_mission_upload_download(wps) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.arm_vehicle() + self.wait_current_waypoint(3) # will be 2km North here + self.change_mode('RTL') + + self.wait_distance_to_home(100, 200, timeout=120) + self.wait_current_waypoint(7) + + self.fly_home_land_and_disarm() + + def send_reposition_to_loc(self, loc): + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, + 1, # reposition flags; 1 means "change to guided" + 0, + 0, + int(loc.lat * 1e7), + int(loc.lng * 1e7), + 20, # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + + def reposition_to_loc(self, loc, accuracy=100): + self.send_reposition_to_loc(loc) + self.wait_location( + loc, + accuracy=accuracy, + minimum_duration=20, + timeout=120, + ) + + def RTL_AUTOLAND_1_FROM_GUIDED(self): + '''test behaviour when RTL_AUTOLAND==1 and entering from guided''' + + self.set_parameters({ + "RTL_AUTOLAND": 1, + }) + + # when RTL is entered and RTL_AUTOLAND is 1 we should fly home + # then to the landing sequence. This mission puts the landing + # sequence well to the West of home so if we go directly there + # we won't come within 200m of home + wps = self.create_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + # fly North + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30), + # add a waypoint 1km North (which we will look for and trigger RTL + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30), + + # *exciting* landing sequence is ~1km West and points away from Home. + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_DO_LAND_START, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5), + (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0), + ]) + self.check_mission_upload_download(wps) + self.set_current_waypoint(0, check_afterwards=False) + + self.change_mode('AUTO') + self.wait_ready_to_arm() + + here = self.mav.location() + guided_loc = self.offset_location_ne(here, 500, -500) + + self.arm_vehicle() + self.wait_current_waypoint(3) # will be 2km North here + self.reposition_to_loc(guided_loc) + self.send_cmd_do_set_mode('RTL') + + self.wait_distance_to_home(100, 200, timeout=120) + self.wait_current_waypoint(7) + + self.fly_home_land_and_disarm() + def tests(self): '''return list of all tests''' @@ -1706,5 +1815,7 @@ def tests(self): self.MAV_CMD_NAV_TAKEOFF, self.Q_GUIDED_MODE, self.DCMClimbRate, + self.RTL_AUTOLAND_1, # as in fly-home then go to landing sequence + self.RTL_AUTOLAND_1_FROM_GUIDED, # as in fly-home then go to landing sequence ]) return ret