diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 3a8f242ab3..8dc5be662b 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -135,7 +135,7 @@ def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1: options=None, logfile=sys.stdout): '''launch mavproxy connected to a SIL instance''' global close_list - MAVPROXY = reltopdir('../MAVProxy/mavproxy.py') + MAVPROXY = os.getenv('MAVPROXY', reltopdir('../MAVProxy/mavproxy.py')) cmd = MAVPROXY + ' --master=%s --out=127.0.0.1:14550' % master if setup: cmd += ' --setup' diff --git a/Tools/run_sim_mission.py b/Tools/run_sim_mission.py new file mode 100644 index 0000000000..523150bbb4 --- /dev/null +++ b/Tools/run_sim_mission.py @@ -0,0 +1,152 @@ +import os.path +import random +import sys +import time + +import pexpect + +MODULE_DIR = os.path.dirname(os.path.realpath(__file__)) + +# This is terrible. +sys.path.insert(0, + os.path.join(MODULE_DIR, '..', '..', 'mavlink', 'pymavlink')) +import mavutil +import mavwp + +# So is this. +sys.path.insert(0, os.path.join(MODULE_DIR, 'autotest')) +sys.path.insert(0, os.path.join(MODULE_DIR, 'autotest', 'pysim')) +import arducopter +import common +import util + +testdir = os.path.dirname(os.path.realpath(__file__)) + + +def error(msg, *args): + sys.stdout.flush() + sys.stderr.write((msg % args) + '\n') + + +def run_mission(mission_path, frame, home, viewerip=None): + sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + sim_cmd += ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % ( + frame, home.lat, home.lng, home.alt, home.heading) + sim_cmd += ' --wind=6,45,.3' + if viewerip: + sim_cmd += ' --fgout=%s:5503' % viewerip + + sil = util.start_SIL('ArduCopter', wipe=True) + mavproxy = util.start_MAVProxy_SIL( + 'ArduCopter', + options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter') + mavproxy.expect('Received [0-9]+ parameters') + + # setup test parameters + mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200)) + mavproxy.send("param load %s/autotest/ArduCopter.parm\n" % testdir) + mavproxy.expect('Loaded [0-9]+ parameters') + mavproxy.send('module load mmap\n') + + # reboot with new parameters + util.pexpect_close(mavproxy) + util.pexpect_close(sil) + + sil = util.start_SIL('ArduCopter', height=home.alt) + print 'Executing command %s' % (sim_cmd,) + sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10) + sim.delaybeforesend = 0 + util.pexpect_autoclose(sim) + options = ('--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter ' + '--streamrate=5') + if viewerip: + options += ' --out=%s:14550' % viewerip + mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) + mavproxy.expect('Logging to (\S+)') + logfile = mavproxy.match.group(1) + print 'Saving log %s' % (logfile,) + + # the received parameters can come before or after the ready to fly message + mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) + mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) + + mavproxy.send('module load mmap\n') + util.expect_setup_callback(mavproxy, common.expect_callback) + + common.expect_list_clear() + common.expect_list_extend([sim, sil, mavproxy]) + + # get a mavlink connection going + try: + mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception, msg: + error("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + mav.message_hooks.append(common.message_hook) + mav.idle_hooks.append(common.idle_hook) + + failed = False + e = 'None' + try: + mav.wait_heartbeat() + arducopter.setup_rc(mavproxy) + + print 'Calibrating level...' + if not arducopter.calibrate_level(mavproxy, mav): + error('Failed to calibrate level.') + failed = True + + print 'Arming motors...' + if not arducopter.arm_motors(mavproxy, mav): + error('Failed to arm motors.') + failed = True + + if (mission_path and + arducopter.load_mission_from_file(mavproxy, mav, mission_path)): + error('Failed to load mission %s' % (mission_path,)) + failed = True + + print 'Taking off.' + if not arducopter.takeoff(mavproxy, mav, 10): + error('Failed to takeoff.') + + if mission_path: + print 'Beginning mission in...' + for i in range(3, 0, -1): + print i + time.sleep(1) + print 'Flying mission.' + if not arducopter.fly_mission(mavproxy, mav, height_accuracy=0.5, + target_altitude=10): + error("fly_mission failed") + failed = True + else: + print 'MISSION COMPLETE.' + + except pexpect.TIMEOUT, e: + error('Command timed out: %s', e) + failed = True + + mav.close() + util.pexpect_close(mavproxy) + util.pexpect_close(sil) + util.pexpect_close(sim) + return not failed + + +def main(): + mission_path = None + if len(sys.argv) > 1: + mission_path = sys.argv[1] + frame = '+' + # If we were given a mission, use its first waypoint as home. + if mission_path: + wploader = mavwp.MAVWPLoader() + wploader.load(mission_path) + wp = wploader.wp(0) + home = mavutil.location(wp.x, wp.y, wp.z, 0) + run_mission(mission_path, frame, home) + + +if __name__ == '__main__': + main()