Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 18:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: copter: add test for RTL_ALT_FINAL

Browse Source
This commit is contained in:
Iampete1 2025-02-09 01:43:02 +00:00 committed by Randy Mackay
parent 2091ca11c3
commit e62ca116df
1 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
Tools/autotest/arducopter.py
View File

@ -9869,6 +9869,47 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.wait_disarmed()
self.reboot_sitl()
def RTL_ALT_FINAL(self):
'''Test RTL with RTL_ALT_FINAL'''
self.progress("arm the vehicle and takeoff in Guided")
self.takeoff(20, mode='GUIDED')
# Set home current location, this gives a large home vs orgin difference
self.set_home(self.mav.location())
self.progress("fly 50m North (or whatever)")
self.fly_guided_move_local(50, 0, 50)
target_alt = 10
self.set_parameter('RTL_ALT_FINAL', target_alt * 100)
self.progress("Waiting RTL to reach Home and hold")
self.change_mode('RTL')
# Expecting to return and hold 10m above home
tstart = self.get_sim_time()
reachedHome = False
while self.get_sim_time_cached() < tstart + 250:
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
home_distance = self.distance_to_home(use_cached_home=True)
home = math.sqrt((alt-target_alt)**2 + home_distance**2) < 2
if home and not reachedHome:
reachedHome = True
self.progress("Reached home - holding")
self.delay_sim_time(20)
continue
if reachedHome:
if not home:
raise NotAchievedException("Should still be at home")
if not self.armed():
raise NotAchievedException("Should still be armed")
break
self.progress("Hold at home successful - landing")
self.change_mode("LAND")
self.wait_landed_and_disarmed()
def SMART_RTL(self):
'''Check SMART_RTL'''
self.progress("arm the vehicle and takeoff in Guided")
@ -12714,6 +12755,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.GSF,
self.GSF_reset,
self.AP_Avoidance,
self.RTL_ALT_FINAL,
self.SMART_RTL,
self.SMART_RTL_EnterLeave,
self.SMART_RTL_Repeat,