/
run_sim_mission.py
152 lines (123 loc) · 4.35 KB
/
run_sim_mission.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
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()