mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-01 13:38:38 -04:00
autotest: check local home location
This commit is contained in:
parent
652b361bc0
commit
635df2ca45
@ -8946,6 +8946,7 @@ class AutoTestCopter(AutoTest):
|
||||
self.TakeoffAlt,
|
||||
self.SplineLastWaypoint,
|
||||
self.Gripper,
|
||||
self.TestLocalHomePosition,
|
||||
self.TestGripperMission,
|
||||
self.VisionPosition,
|
||||
self.ATTITUDE_FAST,
|
||||
|
@ -9553,6 +9553,34 @@ Also, ignores heartbeats not from our target system'''
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
|
||||
def TestLocalHomePosition(self):
|
||||
"""Test local home position is sent in HOME_POSITION message"""
|
||||
self.context_push()
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
# set home to a new location
|
||||
self.mav.mav.command_long_send(1,
|
||||
1,
|
||||
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
-35.357466,
|
||||
149.142589,
|
||||
630)
|
||||
|
||||
# check home after home set
|
||||
m = self.assert_receive_message("HOME_POSITION", timeout=5)
|
||||
if abs(m.x) < 10 or abs(m.y) < 10 or abs(m.z) < 10:
|
||||
raise NotAchievedException("Failed to get local home position: (got=%u, %u, %u)", m.x, m.y, m.z)
|
||||
else:
|
||||
self.progress(f"Received local home position successfully: (got={m.x}, {m.y}, {m.z})")
|
||||
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
|
||||
def install_terrain_handlers_context(self):
|
||||
'''install a message handler into the current context which will
|
||||
listen for an fulfill terrain requests from ArduPilot. Will
|
||||
|
Loading…
Reference in New Issue
Block a user