Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: add test for Plane reposition using terrain offset

Browse Source
This commit is contained in:
Peter Barker 2019-08-05 21:09:07 +10:00 committed by Peter Barker
parent bde43f167e
commit 7bf1a3e978
1 changed files with 56 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
Tools/autotest/arduplane.py
View File

@ -570,11 +570,65 @@ class AutoTestPlane(AutoTest):
p5=int(loc.lat * 1e7),
p6=int(loc.lng * 1e7),
p7=new_alt, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
)
self.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True)
self.fly_home_land_and_disarm()
self.install_terrain_handlers_context()
self.location_offset_ne(loc, 500, 500)
terrain_height_wanted = 150
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
0,
0,
0,
int(loc.lat*1e7),
int(loc.lng*1e7),
terrain_height_wanted, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
)
# move to specific terrain-relative altitude and hold for <n> seconds
tstart = self.get_sim_time_cached()
achieve_start = None
tr = None
while True:
if self.get_sim_time_cached() - tstart > 120:
raise NotAchievedException("Did not move to correct terrain alt")
m = self.mav.recv_match(type='TERRAIN_REPORT',
blocking=True,
timeout=1)
tr = m
terrain_height_achieved = m.current_height
self.progress("terrain_alt=%f want=%f" %
(terrain_height_achieved, terrain_height_wanted))
if m is None:
continue
if abs(terrain_height_wanted - terrain_height_achieved) > 5:
if achieve_start is not None:
self.progress("Achieve stop")
achieve_start = None
elif achieve_start is None:
self.progress("Achieve start")
achieve_start = self.get_sim_time_cached()
if achieve_start is not None:
if self.get_sim_time_cached() - achieve_start > 10:
break
m = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True,
timeout=1)
self.progress("TR: %s" % tr)
self.progress("GPI: %s" % m)
min_delta = 4
delta = abs(m.relative_alt/1000.0 - tr.current_height)
if abs(delta < min_delta):
raise NotAchievedException("Expected altitude delta (want=%f got=%f)" %
(min_delta, delta))
self.fly_home_land_and_disarm(timeout=180)
def ExternalPositionEstimate(self):
'''Test mavlink EXTERNAL_POSITION_ESTIMATE command'''