Browse Source

Tools: autotest: accept home position before or after ack

autopilot may be out of space or time to send home position packet after
we have requested it
master
Peter Barker 6 years ago committed by Peter Barker
parent
commit
6fc4086726
  1. 12
      Tools/autotest/common.py

12
Tools/autotest/common.py

@ -1689,6 +1689,7 @@ class AutoTest(ABC): @@ -1689,6 +1689,7 @@ class AutoTest(ABC):
while True:
if self.get_sim_time() - tstart > 30:
raise NotAchievedException("Failed to poll home position")
self.progress("Sending MAV_CMD_GET_HOME_POSITION")
try:
self.run_cmd(
mavutil.mavlink.MAV_CMD_GET_HOME_POSITION,
@ -1701,10 +1702,13 @@ class AutoTest(ABC): @@ -1701,10 +1702,13 @@ class AutoTest(ABC):
0)
except ValueError as e:
continue
break
m = self.mav.messages.get("HOME_POSITION", None)
if old is not None and m._timestamp == old._timestamp:
raise NotAchievedException("home position not updated")
m = self.mav.messages.get("HOME_POSITION", None)
if m is None:
continue
if old is None:
break
if m._timestamp != old._timestamp:
break
return m
def distance_to_home(self):

Loading…
Cancel
Save