Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: ModeFollow: correct relative altitude being fed to autopilot

Browse Source
This commit is contained in:
Peter Barker 2024-06-06 16:19:15 +10:00 committed by Peter Barker
parent eaf5767bfa
commit 56a865147d
1 changed files with 13 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Tools/autotest/arducopter.py
View File

@ -7568,19 +7568,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_parameter("FENCE_ENABLE", 1)
self.check_avoidance_corners()
def global_position_int_for_location(self, loc, time_boot, heading=0):
return self.mav.mav.global_position_int_encode(
int(time_boot * 1000), # time_boot_ms
int(loc.lat * 1e7),
int(loc.lng * 1e7),
int(loc.alt * 1000), # alt in mm
20, # relative alt - urp.
vx=0,
vy=0,
vz=0,
hdg=heading
)
def ModeFollow(self):
'''Fly follow mode'''
foll_ofs_x = 30 # metres
@ -7610,6 +7597,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
(expected_loc.lat, expected_loc.lng))
self.progress("expected_loc: %s" % str(expected_loc))
origin = self.poll_message('GPS_GLOBAL_ORIGIN')
last_sent = 0
tstart = self.get_sim_time()
while True:
@ -7617,9 +7606,17 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
if now - tstart > 60:
raise NotAchievedException("Did not FOLLOW")
if now - last_sent > 0.5:
gpi = self.global_position_int_for_location(new_loc,
now,
heading=heading)
gpi = self.mav.mav.global_position_int_encode(
int(now * 1000), # time_boot_ms
int(new_loc.lat * 1e7),
int(new_loc.lng * 1e7),
int(new_loc.alt * 1000), # alt in mm
int(new_loc.alt * 1000 - origin.altitude), # relative alt - urp.
vx=0,
vy=0,
vz=0,
hdg=heading
)
gpi.pack(self.mav.mav)
self.mav.mav.send(gpi)
self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)