From 9c3b7e7c7d308184319d07c77af596f218aceab3 Mon Sep 17 00:00:00 2001 From: Ebin Date: Fri, 20 Jul 2018 17:23:31 +0530 Subject: [PATCH] Tools: Autotest for balance bot --- Tools/autotest/autotest.py | 11 +++- Tools/autotest/balancebot.py | 112 +++++++++++++++++++++++++++++++++ Tools/autotest/balancebot1.txt | 19 ++++++ Tools/autotest/common.py | 4 ++ 4 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 Tools/autotest/balancebot.py create mode 100644 Tools/autotest/balancebot1.txt diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index c1deba9939..7c5c644bb3 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -21,6 +21,7 @@ import arducopter import arduplane import ardusub import quadplane +import balancebot from pysim import util from pymavlink import mavutil @@ -229,7 +230,8 @@ __bin_names = { "AntennaTracker": "antennatracker", "CopterAVC": "arducopter-heli", "QuadPlane": "arduplane", - "ArduSub": "ardusub" + "ArduSub": "ardusub", + "balancebot": "ardurover" } @@ -337,6 +339,12 @@ def run_step(step): **fly_opts) return tester.autotest() + if step == 'drive.balancebot': + tester = balancebot.AutoTestBalanceBot(binary, + frame=opts.frame, + **fly_opts) + return tester.autotest() + if step == 'dive.ArduSub': tester = ardusub.AutoTestSub(binary, **fly_opts) return tester.autotest() @@ -630,6 +638,7 @@ if __name__ == "__main__": 'build.APMrover2', 'defaults.APMrover2', 'drive.APMrover2', + 'drive.balancebot', 'build.ArduCopter', 'defaults.ArduCopter', diff --git a/Tools/autotest/balancebot.py b/Tools/autotest/balancebot.py new file mode 100644 index 0000000000..1829398e33 --- /dev/null +++ b/Tools/autotest/balancebot.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +# Drive balancebot in SITL +from __future__ import print_function + +import os +import pexpect + +from apmrover2 import AutoTestRover + +from pymavlink import mavutil + +# get location of scripts +testdir = os.path.dirname(os.path.realpath(__file__)) + +# HOME = mavutil.location(-35.362938, 149.165085, 584, 270) +HOME = mavutil.location(40.071374969556928, + -105.22978898137808, + 1583.702759, + 246) + + +class AutoTestBalanceBot(AutoTestRover): + def __init__(self, + binary, + valgrind=False, + gdb=False, + speedup=10, + frame=None, + params=None, + gdbserver=False, + **kwargs): + super(AutoTestBalanceBot, self).__init__(binary, + valgrind, + gdb, + speedup, + frame, + params, + gdbserver, + **kwargs) + + def init(self): + if self.frame is None: + self.frame = 'balancebot' + super(AutoTestBalanceBot, self).init() + + def drive_mission_balancebot1(self): + self.drive_mission(os.path.join(testdir, "balancebot1.txt")) + + def autotest(self): + """Autotest APMrover2 in SITL.""" + if not self.hasInit: + self.init() + self.progress("Started simulator") + + self.fail_list = [] + try: + self.progress("Waiting for a heartbeat with mavlink protocol %s" % + self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + self.progress("Setting up RC parameters") + self.set_rc_default() + self.set_rc(8, 1800) + self.progress("Waiting for GPS fix") + self.mav.wait_gps_fix() + self.homeloc = self.mav.location() + self.progress("Home location: %s" % self.homeloc) + self.mavproxy.send('switch 6\n') # Manual mode + self.wait_mode('MANUAL') + + self.progress("Waiting reading for arm") + self.wait_ready_to_arm() + self.arm_vehicle() + + self.run_test("Drive an RTL Mission", self.drive_rtl_mission) + + self.run_test("Drive Mission %s" % "balancebot1.txt", + self.drive_mission_balancebot1) + + self.run_test("Disarm Vehicle", self.disarm_vehicle) + + self.run_test("Get Banner", self.do_get_banner) + + self.run_test("Get Capabilities", + self.do_get_autopilot_capabilities) + + self.run_test("Set mode via MAV_COMMAND_DO_SET_MODE", + self.do_set_mode_via_command_long) + + self.run_test("Test ServoRelayEvents", + self.test_servorelayevents) + + self.run_test("Download logs", lambda: + self.log_download( + self.buildlogs_path("APMrover2-log.bin"))) + # if not drive_left_circuit(self): + # self.progress("Failed left circuit") + # failed = True + # if not drive_RTL(self): + # self.progress("Failed RTL") + # failed = True + + except pexpect.TIMEOUT: + self.progress("Failed with timeout") + self.fail_list.append(("*timeout*", None)) + + self.close() + + if len(self.fail_list): + self.progress("FAILED STEPS: %s" % self.fail_list) + return False + return True diff --git a/Tools/autotest/balancebot1.txt b/Tools/autotest/balancebot1.txt new file mode 100644 index 0000000000..bff75bb57a --- /dev/null +++ b/Tools/autotest/balancebot1.txt @@ -0,0 +1,19 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.071289 -105.230057 11085.900391 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071186 -105.230064 9502.200195 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071083 -105.230026 11085.900391 1 +5 0 3 178 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +6 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071011 -105.230003 11085.900391 1 +7 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070999 -105.229919 11085.900391 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070900 -105.229950 11085.900391 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070843 -105.229858 11085.889648 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070801 -105.229820 11085.849609 1 +11 0 3 178 0.000000 2.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +12 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070721 -105.229736 11085.900391 1 +13 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070827 -105.229530 11086.000000 1 +14 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070904 -105.229362 11085.929688 1 +15 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070995 -105.229118 12669.580078 1 +16 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071133 -105.229233 11085.900391 1 +17 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071255 -105.229355 11085.900391 1 +18 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071404 -105.229485 9502.200195 1 +19 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071369 -105.229828 9502.200195 1 diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 58e6759fdc..a7ccb3a160 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -729,6 +729,10 @@ class AutoTest(ABC): def wait_ekf_happy(self, timeout=30): """Wait for EKF to be happy""" + """ if using SITL estimates directly """ + if (int(self.get_parameter('AHRS_EKF_TYPE')) == 10): + return True + tstart = self.get_sim_time() # all of these must be set for arming to happen: required_value = (mavutil.mavlink.EKF_ATTITUDE |