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

Tools: autotest: add test for Rover offboard-in-auto function

Browse Source
This commit is contained in:
Peter Barker 2019-03-15 15:02:57 +11:00 committed by Randy Mackay
parent 27b50f125c
commit 873096f791
3 changed files with 71 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
Tools/autotest/apmrover2.py
View File

@ -891,6 +891,63 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.wait_location(loc, accuracy=accuracy)
self.disarm_vehicle()
def test_offboard(self, timeout=90):
self.load_mission("rover-guided-mission.txt")
self.wait_ready_to_arm(require_absolute=True)
self.arm_vehicle()
self.change_mode("AUTO")
offboard_expected_duration = 10 # see mission file
if self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None):
raise PreconditionFailedException("Already have SET_POSITION_TARGET_GLOBAL_INT")
tstart = self.get_sim_time_cached()
last_heartbeat_sent = 0
got_sptgi = False
magic_waypoint_tstart = 0
magic_waypoint_tstop = 0
while True:
if self.mode_is("HOLD", cached=True):
break
now = self.get_sim_time_cached()
if now - last_heartbeat_sent > 1:
last_heartbeat_sent = now
self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
0,
0,
0)
if now - tstart > timeout:
raise AutoTestTimeoutException("Didn't complete")
magic_waypoint = 3
# mc = self.mav.messages.get("MISSION_CURRENT", None)
mc = self.mav.recv_match(type="MISSION_CURRENT", blocking=False)
if mc is not None:
print("%s" % str(mc))
if mc.seq == magic_waypoint:
print("At magic waypoint")
if magic_waypoint_tstart == 0:
magic_waypoint_tstart = self.get_sim_time_cached()
sptgi = self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None)
if sptgi is not None:
got_sptgi = True
elif mc.seq > magic_waypoint:
if magic_waypoint_tstop == 0:
magic_waypoint_tstop = self.get_sim_time_cached()
self.disarm_vehicle()
offboard_duration = magic_waypoint_tstop - magic_waypoint_tstart
if abs(offboard_duration - offboard_expected_duration) > 1:
raise NotAchievedException("Did not stay in offboard control for correct time (want=%f got=%f)" %
(offboard_expected_duration, offboard_duration))
if not got_sptgi:
raise NotAchievedException("Did not get sptgi message")
print("spgti: %s" % str(sptgi))
def tests(self):
'''return list of all tests'''
ret = super(AutoTestRover, self).tests()
@ -979,6 +1036,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
"Test Rally Points",
self.test_rally_points),
("Offboard",
"Test Offboard Control",
self.test_offboard),
("DataFlashOverMAVLink",
"Test DataFlash over MAVLink",
self.test_dataflash_over_mavlink),

5
Tools/autotest/common.py
View File

@ -1574,8 +1574,9 @@ class AutoTest(ABC):
raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" %
(wpnum_end, wpnum_end))
def mode_is(self, mode):
self.wait_heartbeat()
def mode_is(self, mode, cached=False):
if not cached:
self.wait_heartbeat()
return self.get_mode_from_mode_mapping(self.mav.flightmode) == self.get_mode_from_mode_mapping(mode)
def wait_mode(self, mode, timeout=60):

7
Tools/autotest/rover-guided-mission.txt Normal file
View File

@ -0,0 +1,7 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.0713750 -105.229789 0 1
1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.0713350 -105.229779 0.000000 1
2 0 0 222 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
3 0 0 92 1.000000 0.000000 0.000000 0.000000 39.000000 104.000000 0.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 40.0713550 -105.229769 0.000000 1
5 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1