aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: autotest: add autotest for setting home position with command-int

Browse Source
This commit is contained in:
Peter Barker 2018-07-06 13:29:46 +10:00 committed by Peter Barker
parent fd328f1e13
commit 957f637043
2 changed files with 58 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/autotest/apmrover2.py
View File

@ -881,7 +881,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.arm_vehicle(timeout=5)
self.disarm_vehicle()
success = False
except NotAchievedException as e:
except AutoTestTimeoutException as e:
success = True
pass
self.mav.srcSystem = old_srcSystem

59
Tools/autotest/common.py
View File

@ -973,7 +973,7 @@ class AutoTest(ABC):
tstart = self.get_sim_time_cached()
while True:
if self.get_sim_time_cached() - tstart > timeout:
raise NotAchievedException("Did not get good COMMAND_ACK")
raise AutoTestTimeoutException("Did not get good COMMAND_ACK")
m = self.mav.recv_match(type='COMMAND_ACK',
blocking=True,
timeout=1)
@ -1658,6 +1658,57 @@ class AutoTest(ABC):
self.progress("GroundSpeed OK (got=%f) (want=%f)" %
(m.groundspeed, want))
def fly_test_set_home(self):
self.reboot_sitl()
# HOME_POSITION is used as a surrogate for origin until we
# start emitting GPS_GLOBAL_ORIGIN
orig_home = self.poll_home_position()
if orig_home is None:
raise AutoTestTimeoutException()
self.progress("Original home: %s" % str(orig_home))
new_x = orig_home.latitude + 1000
new_y = orig_home.longitude + 2000
new_z = orig_home.altitude + 300000 # 300 metres
print("new home: %s %s %s" % (str(new_x), str(new_y), str(new_z)))
self.mav.mav.command_int_send(1,
1,
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
0, # current
0, # autocontinue
0, # p1,
0, # p2,
0, # p3,
0, # p4,
new_x,
new_y,
new_z/1000.0) # mm => m
self.expect_command_ack(mavutil.mavlink.MAV_CMD_DO_SET_HOME)
home = self.poll_home_position()
self.progress("int-set home: %s" % str(home))
if (home.latitude != new_x or
home.longitude != new_y or
home.altitude != new_z):
self.reboot_sitl()
raise NotAchievedException(
"(%f, %f, %f) != (%f, %f, %f)" %
(home.latitude, home.longitude, home.altitude),
(new_x, new_y, new_z))
# watch it for a little while to ensure it doesn't drift at all:
tstart = self.get_sim_time()
while self.get_sim_time() - tstart < 10:
home = self.poll_home_position()
self.progress("int-set home: %s" % str(home))
if (home.latitude != new_x or
home.longitude != new_y or
home.altitude != new_z):
self.reboot_sitl()
raise NotAchievedException("home is drifting")
self.reboot_sitl()
def test_arm_feature(self):
"""Common feature to test."""
self.context_push()
@ -2121,7 +2172,11 @@ switch value'''
return {}
def tests(self):
return []
return [
("SetHome",
"Test Set Home",
self.fly_test_set_home),
]
def post_tests_announcements(self):
if self._show_test_timings: