From 38898dc7938d128e05a77037ecad061e0ad1f21c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 29 Jun 2018 20:38:10 +1000 Subject: [PATCH] Tools: autotest: add test for vision position estimate --- Tools/autotest/arducopter.py | 101 ++++++++++++++++++++++++++++++++++- Tools/autotest/common.py | 26 +++++++++ Tools/autotest/pysim/util.py | 5 +- 3 files changed, 130 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a8db7ac311..c0a2658047 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5,6 +5,8 @@ from __future__ import print_function import math import os import shutil +import sys +import time import pexpect from pymavlink import mavutil @@ -91,6 +93,7 @@ class AutoTestCopter(AutoTest): valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver, + vicon=True, wipe=True) self.mavproxy = util.start_MAVProxy_SITL( 'ArduCopter', options=self.mavproxy_options()) @@ -330,7 +333,7 @@ class AutoTestCopter(AutoTest): while self.get_sim_time() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m - pos = self.mav.location() + pos = self.mav.location() # requires a GPS fix to function home_distance = self.get_distance(HOME, pos) home = "" if alt <= 1 and home_distance < 10: @@ -1129,6 +1132,98 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') + def fly_vision_position(self): + '''disable GPS navigation, enable Vicon input''' + # scribble down a location we can set origin to: + + self.progress("Waiting for location") + start = self.mav.location() + self.mavproxy.send('switch 6\n') # stabilize mode + self.mav.wait_heartbeat() + self.wait_mode('STABILIZE') + self.progress("Waiting reading for arm") + self.wait_ready_to_arm() + + old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + print("old_pos=%s" % str(old_pos)) + + old_gps_type = self.get_parameter("GPS_TYPE") + old_ek2_gps_type = self.get_parameter("EK2_GPS_TYPE") + old_serial5_protocol = self.get_parameter("SERIAL5_PROTOCOL") + + ex = None + try: + self.set_parameter("GPS_TYPE", 0) + self.set_parameter("EK2_GPS_TYPE", 3) + self.set_parameter("SERIAL5_PROTOCOL", 1) + self.reboot_sitl() + # without a GPS or some sort of external prompting, AP + # doesn't send system_time messages. So prompt it: + self.mav.mav.system_time_send(time.time() * 1000000, 0) + self.mav.mav.set_gps_global_origin_send(1, + old_pos.lat, + old_pos.lon, + old_pos.alt) + self.progress("Waiting for non-zero-lat") + tstart = self.get_sim_time(); + while True: + gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) +# self.progress("gpi=%s" % str(gpi)) + if gpi.lat != 0: + break + + if self.get_sim_time() - tstart > 10: + raise AutoTestTimeoutException() + + self.takeoff(arm=True) + self.set_rc(1, 1600) + tstart = self.get_sim_time(); + while True: + vicon_pos = self.mav.recv_match(type='VICON_POSITION_ESTIMATE', + blocking=True) +# print("vpe=%s" % str(vicon_pos)) + gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) +# self.progress("gpi=%s" % str(gpi)) + if vicon_pos.x > 40: + break + + if self.get_sim_time() - tstart > 100: + raise AutoTestTimeoutException() + + # recenter controls: + self.set_rc(1, 1500) + self.progress("# Enter RTL") + self.mavproxy.send('switch 3\n') + self.set_rc(3, 1500) + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > 200: + raise NotAchievedException() + gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) +# print("gpi=%s" % str(gpi)) + ss = self.mav.recv_match(type='SIMSTATE', + blocking=True) +# print("ss=%s" % str(ss)) + # wait for RTL disarm: + if not self.armed(): + break + + except Exception as e: + self.progress("Exception caught") + ex = e + + self.set_parameter("GPS_TYPE", old_gps_type) + self.set_parameter("EK2_GPS_TYPE", old_ek2_gps_type) + self.set_parameter("SERIAL5_PROTOCOL", old_serial5_protocol) + self.set_rc(3, 1000) + self.reboot_sitl() + + if ex is not None: + raise ex + def autotest(self): """Autotest ArduCopter in SITL.""" if not self.hasInit: @@ -1143,6 +1238,7 @@ class AutoTestCopter(AutoTest): self.progress("Setting up RC parameters") self.set_rc_default() self.set_rc(3, 1000) + self.progress("Waiting for location") self.homeloc = self.mav.location() self.progress("Home location: %s" % self.homeloc) self.mavproxy.send('switch 6\n') # stabilize mode @@ -1308,6 +1404,9 @@ class AutoTestCopter(AutoTest): # wait for disarm self.mav.motors_disarmed_wait() + '''vision position''' # expects vehicle to be disarmed + self.run_test("Fly Vision Position", self.fly_vision_position) + # Download logs self.run_test("log download", lambda: self.log_download( diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index b17949e7e1..2956ab45d1 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -166,6 +166,32 @@ class AutoTest(ABC): def reboot_sitl(self): self.mavproxy.send("reboot\n") self.mavproxy.expect("tilt alignment complete") + # empty mav to avoid getting old timestamps: + if self.mav is not None: + while self.mav.recv_match(blocking=False): + pass + # after reboot stream-rates may be zero. Prompt MAVProxy to + # send a rate-change message by changing away from our normal + # stream rates and back again: + if self.mav is not None: + tstart = self.get_sim_time() + while True: + + self.mavproxy.send("set streamrate %u\n" % (self.sitl_streamrate()*2)) + if self.mav is None: + break + + if self.get_sim_time() - tstart > 10: + raise AutoTestTimeoutException() + + m = self.mav.recv_match(type='SYSTEM_TIME', + blocking=True, + timeout=1) + if m is not None: + print("Received (%s)" % str(m)) + break; + self.mavproxy.send("set streamrate %u\n" % self.sitl_streamrate()) + self.progress("Reboot complete") def close(self): '''tidy up after running all tests''' diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 1c52359e1f..6f4d841558 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -192,7 +192,8 @@ def valgrind_log_filepath(binary, model): def start_SITL(binary, valgrind=False, gdb=False, wipe=False, synthetic_clock=True, home=None, model=None, speedup=1, defaults_file=None, - unhide_parameters=False, gdbserver=False): + unhide_parameters=False, gdbserver=False, + vicon=False): """Launch a SITL instance.""" cmd = [] if valgrind and os.path.exists('/usr/bin/valgrind'): @@ -236,6 +237,8 @@ def start_SITL(binary, valgrind=False, gdb=False, wipe=False, cmd.extend(['--defaults', defaults_file]) if unhide_parameters: cmd.extend(['--unhide-groups']) + if vicon: + cmd.extend(["--uartF=sim:vicon:"]) print("Running: %s" % cmd_as_shell(cmd)) first = cmd[0] rest = cmd[1:]