From d43fc591c3e7d32dcf943a248abdb9d5fa61c3c1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 6 Apr 2020 14:47:54 +1000 Subject: [PATCH] autotest: add skidsteer test --- Tools/autotest/apmrover2.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 312cf40540..14fca499e2 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -11,6 +11,7 @@ import time from common import AutoTest from pysim import util +from pysim import vehicleinfo from common import AutoTestTimeoutException from common import MsgRcvTimeoutException @@ -4927,6 +4928,33 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) raise NotAchievedException("Did not get correct command_id") break + def test_skid_steer(self): + model = "rover-skid" + vinfo = vehicleinfo.VehicleInfo() + defaults_filepath = vinfo.options["APMrover2"]["frames"][model]["default_params_filename"] + self.customise_SITL_commandline([], + model=model, + defaults_file=defaults_filepath) + self.change_mode("MANUAL") + self.wait_ready_to_arm() + self.arm_vehicle() + self.progress("get a known heading to avoid worrying about wrap") + # this is steering-type-two-paddles + self.set_rc(1, 1400) + self.set_rc(3, 1500) + self.wait_heading(90) + self.progress("straighten up") + self.set_rc(1, 1500) + self.set_rc(3, 1500) + self.progress("steer one way") + self.set_rc(1, 1600) + self.set_rc(3, 1400) + self.wait_heading(120) + self.progress("steer the other") + self.set_rc(1, 1400) + self.set_rc(3, 1600) + self.wait_heading(60) + def tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -5057,6 +5085,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) "Test DataFlash SITL backend", self.test_dataflash_sitl), + ("SkidSteer", + "Check skid-steering", + self.test_skid_steer), + ("PolyFence", "PolyFence tests", self.test_poly_fence),