Tools: Autotest for balance bot

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',

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

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

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 |