def refresh(self):
# open port and request all parameters
inmaster = mavutil.mavlink_connection(self.indevice, input=False)
inmaster.target_system = 20
inmaster.param_fetch_all()
inmaster.close()
# wait until all parameters received
outmaster = mavutil.mavlink_connection(self.outdevice)
outmaster.port.settimeout(2)
t = time.time()
while True:
try:
m = outmaster.recv_msg()
except socket.timeout:
outmaster.close()
return TIMEOUT
if t + 2 < time.time():
outmaster.close()
print "timeout"
return TIMEOUT
if m is not None:
if type(m) == mavlink.MAVLink_param_value_message:
t = time.time()
self.param_value_received.emit(m)
# print m.param_count, "/", m.param_index, m.param_id, m.param_value
# print m.param_id.split('_')[0]
if m.param_count == m.param_index + 1:
outmaster.close()
return LAST_VALUE
def refresh(self):
# open port and request all parameters
inmaster = mavutil.mavlink_connection(self.indevice, input=False)
inmaster.target_system = 20
inmaster.param_fetch_all()
inmaster.close()
# wait until all parameters received
outmaster = mavutil.mavlink_connection(self.outdevice)
outmaster.port.settimeout(2)
t = time.time()
while True:
try:
m = outmaster.recv_msg()
except socket.timeout:
outmaster.close()
return TIMEOUT
if t + 2 < time.time():
outmaster.close()
print "timeout"
return TIMEOUT
if m is not None:
if type(m) == mavlink.MAVLink_param_value_message:
t = time.time()
self.param_value_received.emit(m)
# print m.param_count, "/", m.param_index, m.param_id, m.param_value
# print m.param_id.split('_')[0]
if m.param_count == m.param_index + 1:
outmaster.close()
return LAST_VALUE
def __init__(self, filename):
self.root = Tkinter.Tk()
self.filesize = os.path.getsize(filename)
self.filepos = 0.0
self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
robust_parsing=True)
self.mout = []
for m in opts.out:
self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate))
self.fgout = []
for f in opts.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is None:
sys.exit(1)
self.last_timestamp = getattr(self.msg, '_timestamp')
self.paused = False
self.topframe = Tkinter.Frame(self.root)
self.topframe.pack(side=Tkinter.TOP)
self.frame = Tkinter.Frame(self.root)
self.frame.pack(side=Tkinter.LEFT)
self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
orient=Tkinter.HORIZONTAL, command=self.slew)
self.slider.pack(side=Tkinter.LEFT)
self.clock = Tkinter.Label(self.topframe,text="")
self.clock.pack(side=Tkinter.RIGHT)
self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
self.playback.pack(side=Tkinter.BOTTOM)
self.playback.delete(0, "end")
self.playback.insert(0, 1)
self.buttons = {}
self.button('quit', 'gtk-quit.gif', self.frame.quit)
self.button('pause', 'media-playback-pause.gif', self.pause)
self.button('rewind', 'media-seek-backward.gif', self.rewind)
self.button('forward', 'media-seek-forward.gif', self.forward)
self.button('status', 'Status', self.status)
self.flightmode = Tkinter.Label(self.frame,text="")
self.flightmode.pack(side=Tkinter.RIGHT)
self.next_message()
self.root.mainloop()
def playback(filename, images):
'''playback one file'''
mlog = mavutil.mavlink_connection(filename, robust_parsing=True)
mout = mavutil.mavlink_connection(opts.out, input=False, baud=opts.baudrate)
# get first message
msg = mlog.recv_match(condition=opts.condition)
last_timestamp = msg._timestamp
last_print = time.time()
# skip any older images
while len(images) and images[0].frame_time < msg._timestamp:
images.pop(0)
params = []
param_send = []
while True:
msg = mlog.recv_match(condition=opts.condition)
if msg is None:
return
if msg.get_type() == 'PARAM_VALUE':
params.append(msg)
mout.write(msg.get_msgbuf())
deltat = msg._timestamp - last_timestamp
if len(images) == 0 or images[0].frame_time > msg._timestamp + 2:
# run at high speed except for the portions where we have images
deltat /= 60
time.sleep(max(min(deltat/opts.speedup, 5), 0))
last_timestamp = msg._timestamp
if time.time() - last_print > 2.0:
print('%s' % (time.asctime(time.localtime(msg._timestamp))))
last_print = time.time()
if len(images) and msg._timestamp > images[0].frame_time:
img = images.pop(0)
try:
os.unlink('fake_chameleon.tmp')
except Exception:
pass
os.symlink(img.filename, 'fake_chameleon.tmp')
os.rename('fake_chameleon.tmp', 'fake_chameleon.pgm')
print(img.filename)
# check for parameter fetch messages
msg = mout.recv_msg()
if msg and msg.get_type() == 'PARAM_REQUEST_LIST':
print("Sending %u parameters" % len(params))
param_send = params[:]
if len(param_send) != 0:
p = param_send.pop(0)
mout.write(p.get_msgbuf())
def sigloss(logfile):
'''work out signal loss times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
last_t = 0
types = opts.types
if types is not None:
types = types.split(',')
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
return
if types is not None and m.get_type() not in types:
continue
if opts.notimestamps:
if not 'usec' in m._fieldnames:
continue
t = m.usec / 1.0e6
else:
t = m._timestamp
if last_t != 0:
if t - last_t > opts.deltat:
print("Sig lost for %.1fs at %s" %
(t - last_t, time.asctime(time.localtime(t))))
last_t = t
def flight_time(logfile):
'''work out flight time for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
in_air = False
start_time = 0.0
total_time = 0.0
t = None
while True:
m = mlog.recv_match(type='VFR_HUD', condition=opts.condition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60))
return total_time
t = time.localtime(m._timestamp)
if m.groundspeed > opts.groundspeed and not in_air:
print("In air at %s (percent %.0f%% groundspeed %.1f)" % (time.asctime(t), mlog.percent, m.groundspeed))
in_air = True
start_time = time.mktime(t)
elif m.groundspeed < opts.groundspeed and in_air:
print("On ground at %s (percent %.1f%% groundspeed %.1f time=%.1f seconds)" % (
time.asctime(t), mlog.percent, m.groundspeed, time.mktime(t) - start_time))
in_air = False
total_time += time.mktime(t) - start_time
return total_time
def input(q, e_pause, e_kill, device):#{{{
""" Менеджер входящих сообщений.
q -- очередь сообщений, в которую надо складывать успешно принятые пакеты
device -- сетевой сокет, из которого сыпятся байты, в идеале содержащие пакеты """
# ждем, пока нас снимут с паузы
dbgprint("**** link input thread ready")
e_pause.wait()
dbgprint("**** link input thread run")
master = mavutil.mavlink_connection(device)
master.port.settimeout(1)
m = None
while True:
if e_kill.is_set():
dbgprint("**** Link input thread. Sigterm received. Exiting")
return
try: m = master.recv_msg()
except socket.timeout: pass
if m is not None:
# print type(m)
try: q.put_nowait(m)
except Full: pass
def transmit(self):
inmaster = mavutil.mavlink_connection(self.indevice, input=False)
inmaster.target_system = 20
i = 0
for an in self.grid:
param_id = "AN_ch" + str(i + 1) + "_c1"
v = int(self.grid[i].lineedit_c1.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
param_id = "AN_ch" + str(i + 1) + "_c2"
v = int(self.grid[i].lineedit_c2.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
param_id = "AN_ch" + str(i + 1) + "_c3"
v = int(self.grid[i].lineedit_c3.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
i += 1
print "saved AN", i
# param_id = "AN_ch" + str(i + 1) + "_c3"
# v = int(self.grid[i].lineedit_c3.text())
# inmaster.mav.param_set_send(20, 0, param_id, v, 6)
# time.sleep(0.05)
inmaster.close()
def drive_APMrover2(viewerip=None):
'''drive APMrover2 in SIL
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the mission in real time
'''
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
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/Rover.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Starting at lat')
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def flight_time(logfile):
'''work out flight time for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
in_air = False
start_time = 0.0
total_time = 0.0
t = None
while True:
m = mlog.recv_match(type='VFR_HUD')
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60))
return total_time
t = time.localtime(m._timestamp)
if m.groundspeed > 3.0 and not in_air:
print("In air at %s" % time.asctime(t))
in_air = True
start_time = time.mktime(t)
elif m.groundspeed < 3.0 and in_air:
print("On ground at %s" % time.asctime(t))
in_air = False
total_time += time.mktime(t) - start_time
return total_time
def process(filename):
'''process one logfile'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
output = None
count = 1
dirname = os.path.dirname(filename)
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if mlog.flightmode.upper() == opts.mode.upper():
if output is None:
path = os.path.join(dirname, "%s%u.log" % (opts.mode, count))
count += 1
print("Creating %s" % path)
output = mavutil.mavlogfile(path, write=True)
else:
if output is not None:
output.close()
output = None
if output and m.get_type() != 'BAD_DATA':
timestamp = getattr(m, '_timestamp', None)
output.write(struct.pack('>Q', timestamp*1.0e6))
output.write(m.get_msgbuf())
def transmit(self):
inmaster = mavutil.mavlink_connection(self.indevice, input=False)
inmaster.target_system = 20
i = 0
for an in self.grid:
param_id = "AN_ch" + str(i + 1) + "_c1"
v = int(self.grid[i].lineedit_c1.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
param_id = "AN_ch" + str(i + 1) + "_c2"
v = int(self.grid[i].lineedit_c2.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
param_id = "AN_ch" + str(i + 1) + "_c3"
v = int(self.grid[i].lineedit_c3.text())
inmaster.mav.param_set_send(20, 0, param_id, v, 6)
time.sleep(0.05)
i += 1
print "saved AN", i
# param_id = "AN_ch" + str(i + 1) + "_c3"
# v = int(self.grid[i].lineedit_c3.text())
# inmaster.mav.param_set_send(20, 0, param_id, v, 6)
# time.sleep(0.05)
inmaster.close()
def input(q, e_pause, e_kill, device): #{{{
""" Менеджер входящих сообщений.
q -- очередь сообщений, в которую надо складывать успешно принятые пакеты
device -- сетевой сокет, из которого сыпятся байты, в идеале содержащие пакеты """
# ждем, пока нас снимут с паузы
dbgprint("**** link input thread ready")
e_pause.wait()
dbgprint("**** link input thread run")
master = mavutil.mavlink_connection(device)
master.port.settimeout(1)
m = None
while True:
if e_kill.is_set():
dbgprint("**** Link input thread. Sigterm received. Exiting")
return
try:
m = master.recv_msg()
except socket.timeout:
pass
if m is not None:
# print type(m)
try:
q.put_nowait(m)
except Full:
pass
def fly_ArduCopter_scripted(testname):
'''fly ArduCopter in SIL
'''
global homeloc
sim_cmd = util.reltopdir(
'Tools/autotest/pysim/sim_multicopter.py'
) + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
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/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
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'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
# 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'])
util.expect_setup_callback(mavproxy, arducopter.expect_callback)
expect_list_clear()
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:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def sigloss(logfile):
'''work out signal loss times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
last_t = 0
types = opts.types
if types is not None:
types = types.split(',')
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
return
if types is not None and m.get_type() not in types:
continue
if opts.notimestamps:
if not 'usec' in m._fieldnames:
continue
t = m.usec / 1.0e6
else:
t = m._timestamp
if last_t != 0:
if t - last_t > opts.deltat:
print("Sig lost for %.1fs at %s" % (t-last_t, time.asctime(time.localtime(t))))
last_t = t
def flight_time(logfile):
'''work out flight time for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
in_air = False
start_time = 0.0
total_time = 0.0
t = None
while True:
m = mlog.recv_match(type='VFR_HUD', condition=opts.condition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time : %u:%02u" %
(int(total_time) / 60, int(total_time) % 60))
return total_time
t = time.localtime(m._timestamp)
if m.groundspeed > opts.groundspeed and not in_air:
print("In air at %s (groundspeed %.1f)" %
(time.asctime(t), m.groundspeed))
in_air = True
start_time = time.mktime(t)
elif m.groundspeed < opts.groundspeed and in_air:
print(
"On ground at %s (groundspeed %.1f time=%.1f seconds)" %
(time.asctime(t), m.groundspeed, time.mktime(t) - start_time))
in_air = False
total_time += time.mktime(t) - start_time
return total_time
def drive_APMrover2(viewerip=None):
'''drive APMrover2 in SIL
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the mission in real time
'''
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
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/Rover.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Starting at lat')
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def process(filename):
'''process one logfile'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
output = None
count = 1
dirname = os.path.dirname(filename)
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if mlog.flightmode.upper() == opts.mode.upper():
if output is None:
path = os.path.join(dirname, "%s%u.log" % (opts.mode, count))
count += 1
print("Creating %s" % path)
output = mavutil.mavlogfile(path, write=True)
else:
if output is not None:
output.close()
output = None
if output and m.get_type() != 'BAD_DATA':
timestamp = getattr(m, '_timestamp', None)
output.write(struct.pack('>Q', timestamp * 1.0e6))
output.write(m.get_msgbuf())
def __init__(self, parent, out_queue, sysid=39):
threading.Thread.__init__(self)
self.sysid = sysid
# use dest port 32002, sysid 39
# use dest port 32001, sysid 43
self.mavs = mavutil.mavlink_connection("udp:127.0.0.1:17779", input=True, source_system=self.sysid)
self.mavo = mavutil.mavlink_connection("udp:127.0.0.1:32001", input=False, source_system=self.sysid)
# self.mav2 = mavutil.mavlink_connection("udp:127.0.0.1:32002", input=False, source_system=self.sysid)
self.mavqgc = mavutil.mavlink_connection("udp:127.0.0.1:14550", input=False, source_system=self.sysid)
# self.mavs = mavutil.mavlink_connection("udp:127.0.0.1:17779", input=True, source_system=self.sysid)
# self.mavo = mavutil.mavlink_connection("udp:127.0.0.1:32002", input=False, source_system=self.sysid)
self.out_queue = out_queue
self.running = True
self.daemon = True
self.parent = parent
print "DS isDaemon", self.isDaemon()
def init_mavlink(self):
# create a mavlink instance
self.mav1 = mavutil.mavlink_connection("COM21", 57600,10)
print("Waiting for HEARTBEAT")
#self.mav1.wait_heartbeat()
#print("Heartbeat from APM (system %u component %u)" % (self.mav1.target_system, self.mav1.target_component))
self.mav1.target_system = 1
self.mav1.target_component = 1
def write_rom():
inmaster = mavutil.mavlink_connection(indevice, input=False)
inmaster.target_system = 20
v = 32767
param_id = "REL_Z_0"
inmaster.mav.param_set_send(20, 0, param_id, v, 5)
time.sleep(0.1)
def write_rom():
inmaster = mavutil.mavlink_connection(indevice, input=False)
inmaster.target_system = 20
v = 32767
param_id = "REL_Z_0"
inmaster.mav.param_set_send(20, 0, param_id, v, 5)
time.sleep(0.1)
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
path = []
while True:
m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT'])
if m is None:
break
if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition(opts.condition):
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
if lat != 0 or lng != 0:
if mlog.flightmode in colourmap:
point = (lat, lng, colourmap[mlog.flightmode])
else:
point = (lat, lng)
path.append(point)
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
if len(path) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path)
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath',
linewidth=2, colour=(255,0,180))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission',
linewidth=2, colour=(255,255,255))
else:
mission_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_obj, mission_obj)
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon)
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
def process(logfile):
"""display accel cal for a log file"""
mlog = mavutil.mavlink_connection(
filename, planner_format=opts.planner, notimestamps=opts.notimestamps, robust_parsing=opts.robust
)
m = mlog.recv_match(type="SENSOR_OFFSETS")
if m is not None:
z_sensor = (m.accel_cal_z - 9.805) * (4096 / 9.81)
print("accel cal %5.2f %5.2f %5.2f %6u %s" % (m.accel_cal_x, m.accel_cal_y, m.accel_cal_z, z_sensor, logfile))
def fly_ArduCopter_scripted(testname):
'''fly ArduCopter in SIL
'''
global homeloc
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
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/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
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'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
# 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'])
util.expect_setup_callback(mavproxy, arducopter.expect_callback)
expect_list_clear()
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:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
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
def mavparms(logfile):
'''extract mavlink parameters'''
mlog = mavutil.mavlink_connection(filename)
while True:
m = mlog.recv_match(type='PARAM_VALUE')
if m is None:
return
pname = str(m.param_id).strip()
if len(pname) > 0:
parms[pname] = m.param_value
def __init__(self):
'''
Constructor
'''
QtCore.QThread.__init__(self)
self.exiting = False
udp_ip = ground_config.get("remote_udp", "ip")
udp_port = ground_config.get("remote_udp", "port")
self._link = mavutil.mavlink_connection("udp:{}:{}".format(udp_ip, udp_port), input=False, source_system=1)
# send initial heartbeat to get things rolling
self.send_heartbeat()
def mavparms(logfile):
'''extract mavlink parameters'''
mlog = mavutil.mavlink_connection(filename)
while True:
m = mlog.recv_match(type='PARAM_VALUE')
if m is None:
return
pname = str(m.param_id).strip()
if len(pname) > 0:
parms[pname] = m.param_value
def process(logfile):
'''display accel cal for a log file'''
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
m = mlog.recv_match(type='SENSOR_OFFSETS')
if m is not None:
z_sensor = (m.accel_cal_z - 9.805) * (4096 / 9.81)
print("accel cal %5.2f %5.2f %5.2f %6u %s" %
(m.accel_cal_x, m.accel_cal_y, m.accel_cal_z, z_sensor, logfile))
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
data = []
last_t = 0
offsets = Vector3(0,0,0)
# now gather all the data
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if m.get_type() == "SENSOR_OFFSETS":
# update current offsets
offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
if m.get_type() == "RAW_IMU":
mag = Vector3(m.xmag, m.ymag, m.zmag)
# add data point after subtracting the current offsets
data.append(mag - offsets + noise())
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
data = select_data(data)
# remove initial outliers
data.sort(lambda a,b : radius_cmp(a,b,offsets))
data = data[len(data)/16:-len(data)/16]
# do an initial fit
(offsets, field_strength) = fit_data(data)
for count in range(3):
# sort the data by the radius
data.sort(lambda a,b : radius_cmp(a,b,offsets))
print("Fit %u : %s field_strength=%6.1f to %6.1f" % (
count, offsets,
radius(data[0], offsets), radius(data[-1], offsets)))
# discard outliers, keep the middle 3/4
data = data[len(data)/8:-len(data)/8]
# fit again
(offsets, field_strength) = fit_data(data)
print("Final : %s field_strength=%6.1f to %6.1f" % (
offsets,
radius(data[0], offsets), radius(data[-1], offsets)))
def process_file(filename):
'''process one file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
vars = {}
while True:
msg = mlog.recv_match(opts.condition)
if msg is None: break
tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
tdays += 719163 # pylab wants it since 0001-01-01
add_data(tdays, msg, mlog.messages)
def process_file(filename):
'''process one file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
vars = {}
while True:
msg = mlog.recv_match(opts.condition)
if msg is None: break
tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
tdays += 719163 # pylab wants it since 0001-01-01
add_data(tdays, msg, mlog.messages)
def run_location(self):
while serial_to_Arduino.globalvar_connection.connection.trulyConnectedAfterReceivingResponse == False:
print(
"telem is waiting for Arduino to connect before trying to connect"
)
time.sleep(1)
arduinoserport = str(
serial_to_Arduino.globalvar_connection.connection.ser.port)
if arduinoserport in self.possibleSerPorts:
self.possibleSerPorts.remove(arduinoserport)
print("Since arduino connected on " + arduinoserport +
", will try to connect telem on other ports: " +
str(self.possibleSerPorts))
keeptrying = True
while keeptrying:
try:
self.conn = mavutil.mavlink_connection(self.serport,
baud=57600)
time.sleep(1)
keeptrying = False
print("telemetry was plugged into \'" + self.serport + "\'")
except serial.serialutil.SerialException:
print("waiting for telemetry USB to be plugged in...")
time.sleep(1)
# if we can't find it on USB0, try other USB ports
self.possibleSerPortIdxCheck += 1
if self.possibleSerPortIdxCheck == len(self.possibleSerPorts):
self.possibleSerPortIdxCheck = 0
self.serport = self.possibleSerPorts[
self.possibleSerPortIdxCheck]
print("Telemetry USB was connected successfully")
self.ispluggedinlocker.acquire()
self.is_plugged_in_and_working = True
self.ispluggedinlocker.release()
print "Set CALLBACK"
self.conn.mav.set_callback(self.get_gps_callback)
print "Getting mavlink Connection:"
while True:
location = self.conn.location()
#location = mavutil.location(37.235,39.323,400,20, 90)
self.lock.acquire()
self.current = {
"lat": location.lat,
"lng": location.lng,
"alt": location.alt,
"rel_alt": location.rel_alt,
"heading": location.heading
}
self.lock.release()
time.sleep(0.01)
def mav_to_gpx(infilename, outfilename):
'''convert a mavlink log file to a GPX file'''
mlog = mavutil.mavlink_connection(infilename)
outf = open(outfilename, mode='w')
def process_packet(m):
t = time.localtime(m._timestamp)
outf.write('''<trkpt lat="%s" lon="%s">
<ele>%s</ele>
<time>%s</time>
<course>%s</course>
<speed>%s</speed>
<fix>3d</fix>
</trkpt>
''' % (m.lat, m.lon, m.alt,
time.strftime("%Y-%m-%dT%H:%M:%SZ", t),
m.hdg, m.v))
def add_header():
outf.write('''<?xml version="1.0" encoding="UTF-8"?>
<gpx
version="1.0"
creator="pymavlink"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xmlns="http://www.topografix.com/GPX/1/0"
xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
<trk>
<trkseg>
''')
def add_footer():
outf.write('''</trkseg>
</trk>
</gpx>
''')
add_header()
count=0
while True:
m = mlog.recv_match(type='GPS_RAW', condition=opts.condition)
if m is None: break
if m.fix_type != 2 and not opts.nofixcheck:
continue
if m.lat == 0.0 or m.lon == 0.0:
continue
process_packet(m)
count += 1
add_footer()
print("Created %s with %u points" % (outfilename, count))
def __init__(self):
'''
Constructor
'''
QtCore.QThread.__init__(self)
self.exiting = False
udp_ip = ground_config.get("remote_udp", "ip")
udp_port = ground_config.get("remote_udp", "port")
self._link = mavutil.mavlink_connection("udp:{}:{}".format(
udp_ip, udp_port),
input=False,
source_system=1)
# send initial heartbeat to get things rolling
self.send_heartbeat()
def mav_to_gpx(infilename, outfilename):
'''convert a mavlink log file to a GPX file'''
mlog = mavutil.mavlink_connection(infilename)
outf = open(outfilename, mode='w')
def process_packet(m):
t = time.localtime(m._timestamp)
outf.write('''<trkpt lat="%s" lon="%s">
<ele>%s</ele>
<time>%s</time>
<course>%s</course>
<speed>%s</speed>
<fix>3d</fix>
</trkpt>
''' % (m.lat, m.lon, m.alt, time.strftime("%Y-%m-%dT%H:%M:%SZ",
t), m.hdg, m.v))
def add_header():
outf.write('''<?xml version="1.0" encoding="UTF-8"?>
<gpx
version="1.0"
creator="pymavlink"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xmlns="http://www.topografix.com/GPX/1/0"
xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
<trk>
<trkseg>
''')
def add_footer():
outf.write('''</trkseg>
</trk>
</gpx>
''')
add_header()
count = 0
while True:
m = mlog.recv_match(type='GPS_RAW', condition=opts.condition)
if m is None: break
if m.fix_type != 2 and not opts.nofixcheck:
continue
if m.lat == 0.0 or m.lon == 0.0:
continue
process_packet(m)
count += 1
add_footer()
print("Created %s with %u points" % (outfilename, count))
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
flying = False
gps_heading = 0.0
data = []
# get the current mag offsets
m = mlog.recv_match(type='SENSOR_OFFSETS')
offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
attitude = mlog.recv_match(type='ATTITUDE')
# now gather all the data
while True:
m = mlog.recv_match()
if m is None:
break
if m.get_type() == "GPS_RAW":
# flying if groundspeed more than 5 m/s
flying = (m.v > opts.minspeed and m.fix_type == 2)
gps_heading = m.hdg
if m.get_type() == "GPS_RAW_INT":
# flying if groundspeed more than 5 m/s
flying = (m.vel/100 > opts.minspeed and m.fix_type == 3)
gps_heading = m.cog/100
if m.get_type() == "ATTITUDE":
attitude = m
if m.get_type() == "SENSOR_OFFSETS":
# update current offsets
offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
if not flying:
continue
if m.get_type() == "RAW_IMU":
data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
ofs2 = fit_data(data)
print("Declination estimate: %.1f" % ofs2[-1])
new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
a = [[ofs2[3], ofs2[4], ofs2[5]],
[ofs2[6], ofs2[7], ofs2[8]],
[ofs2[9], ofs2[10], ofs2[11]]]
print(a)
print("New offsets : %s" % new_offsets)
def mavloss(logfile):
'''work out signal loss times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
m = mlog.recv_match(condition=opts.condition)
while True:
m = mlog.recv_match()
if m is None:
break
print("%u packets, %u lost %.1f%%" % (
mlog.mav_count, mlog.mav_loss, mlog.packet_loss()))
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
if mlog.mavlink10():
m = mlog.recv_match(type='MISSION_ITEM')
else:
m = mlog.recv_match(type='WAYPOINT')
if m is None:
break
wp.set(m, m.seq)
wp.save(opts.output)
print("Saved %u waypoints to %s" % (wp.count(), opts.output))
def mavloss(logfile):
'''work out signal loss times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust)
m = mlog.recv_match(condition=opts.condition)
while True:
m = mlog.recv_match()
if m is None:
break
print("%u packets, %u lost %.1f%%" %
(mlog.mav_count, mlog.mav_loss, mlog.packet_loss()))
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
if mlog.mavlink10():
m = mlog.recv_match(type='MISSION')
else:
m = mlog.recv_match(type='WAYPOINT')
if m is None:
break
wp.set(m, m.seq)
wp.save(opts.output)
print("Saved %u waypoints to %s" % (wp.count(), opts.output))
def mavsearch(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
if opts.types is not None:
types = opts.types.split(',')
else:
types = None
while True:
m = mlog.recv_match(type=types)
if m is None:
break
if mlog.check_condition(opts.condition):
print m
if opts.stopcondition:
break
if opts.stop:
break
def mavsearch(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
if opts.types is not None:
types = opts.types.split(',')
else:
types = None
while True:
m = mlog.recv_match(type=types)
if m is None:
break
if mlog.check_condition(opts.condition):
print m
if opts.stopcondition:
break
if opts.stop:
break
def flight_modes(logfile):
'''show flight modes for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
mode = -1
nav_mode = -1
filesize = os.path.getsize(filename)
while True:
m = mlog.recv_match(type=['SYS_STATUS', 'HEARTBEAT'],
condition='MAV.flightmode!="%s"' % mlog.flightmode)
if m is None:
return
pct = (100.0 * mlog.f.tell()) / filesize
print('%s MAV.flightmode=%-12s (MAV.timestamp=%u %u%%)' %
(time.asctime(time.localtime(
m._timestamp)), mlog.flightmode, m._timestamp, pct))
def lock_time(logfile):
'''work out gps lock times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
locked = False
start_time = 0.0
total_time = 0.0
t = None
m = mlog.recv_match(type=['GPS_RAW_INT', 'GPS_RAW'],
condition=opts.condition)
if m is None:
return 0
unlock_time = time.mktime(time.localtime(m._timestamp))
while True:
m = mlog.recv_match(type=['GPS_RAW_INT', 'GPS_RAW'],
condition=opts.condition)
if m is None:
if locked:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Lock time : %u:%02u" %
(int(total_time) / 60, int(total_time) % 60))
return total_time
t = time.localtime(m._timestamp)
if m.fix_type >= 2 and not locked:
print("Locked at %s after %u seconds" %
(time.asctime(t), time.mktime(t) - unlock_time))
locked = True
start_time = time.mktime(t)
elif m.fix_type == 1 and locked:
print("Lost GPS lock at %s" % time.asctime(t))
locked = False
total_time += time.mktime(t) - start_time
unlock_time = time.mktime(t)
elif m.fix_type == 0 and locked:
print("Lost protocol lock at %s" % time.asctime(t))
locked = False
total_time += time.mktime(t) - start_time
unlock_time = time.mktime(t)
return total_time
def flight_modes(logfile):
'''show flight modes for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
mode = -1
nav_mode = -1
while True:
m = mlog.recv_match(type='SYS_STATUS',
condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' % (mode, nav_mode))
if m is None:
return
mode = m.mode
nav_mode = m.nav_mode
print('%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u)' % (
time.asctime(time.localtime(m._timestamp)),
mlog.flightmode,
mode, nav_mode, m._timestamp))
def flight_modes(logfile):
'''show flight modes for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
mode = -1
nav_mode = -1
filesize = os.path.getsize(filename)
while True:
m = mlog.recv_match(type=['SYS_STATUS','HEARTBEAT'],
condition='MAV.flightmode!="%s"' % mlog.flightmode)
if m is None:
return
pct = (100.0 * mlog.f.tell()) / filesize
print('%s MAV.flightmode=%-12s (MAV.timestamp=%u %u%%)' % (
time.asctime(time.localtime(m._timestamp)),
mlog.flightmode,
m._timestamp, pct))
def main():
port, baud = '/dev/ttyACM0', 57600
parser = ArgumentParser(description = 'Aquisição de dados PX4Flow')
parser.add_argument('--device', '-d', action = 'store', dest = 'port',
default = '/dev/ttyACM0', required = False,
help = 'Configura porta serial (padrão: /dev/ttyACM0)')
parser.add_argument('--baud', '-b', action = 'store', dest = 'baud',
default = 57600, required = False,
help = 'Configura baud rate (padrão: 57600)')
arguments = parser.parse_args()
mavDisp = mavutil.mavlink_connection(port, baud)
mavDisp.wait_heartbeat()
print("Heartbeat from APM (system %u component %u)\n" % (mavDisp.target_system, mavDisp.target_component ))
cin = ''
while cin != 's':
msg = mavDisp.recv_msg()
# print msg
if msg is None:
print "NoneType"
elif msg.get_type() == 'BAD_DATA' or msg.get_type() == 'ENCAPSULATED_DATA':
print msg.get_type()
else:
print '\n'
dic = msg.to_dict()
for field in msg.get_fieldnames():
print field, ': ', dic[field]
cin = raw_input('\nSair? (s/n): ')
def flight_modes(logfile):
'''show flight modes for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
mode = -1
nav_mode = -1
while True:
m = mlog.recv_match(
type='SYS_STATUS',
condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' %
(mode, nav_mode))
if m is None:
return
mode = m.mode
nav_mode = m.nav_mode
print(
'%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u)' %
(time.asctime(time.localtime(
m._timestamp)), mlog.flightmode, mode, nav_mode, m._timestamp))
def lock_time(logfile):
'''work out gps lock times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
locked = False
start_time = 0.0
total_time = 0.0
t = None
m = mlog.recv_match(type=['GPS_RAW_INT','GPS_RAW'], condition=opts.condition)
if m is None:
return 0
unlock_time = time.mktime(time.localtime(m._timestamp))
while True:
m = mlog.recv_match(type=['GPS_RAW_INT','GPS_RAW'], condition=opts.condition)
if m is None:
if locked:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Lock time : %u:%02u" % (int(total_time)/60, int(total_time)%60))
return total_time
t = time.localtime(m._timestamp)
if m.fix_type >= 2 and not locked:
print("Locked at %s after %u seconds" % (time.asctime(t),
time.mktime(t) - unlock_time))
locked = True
start_time = time.mktime(t)
elif m.fix_type == 1 and locked:
print("Lost GPS lock at %s" % time.asctime(t))
locked = False
total_time += time.mktime(t) - start_time
unlock_time = time.mktime(t)
elif m.fix_type == 0 and locked:
print("Lost protocol lock at %s" % time.asctime(t))
locked = False
total_time += time.mktime(t) - start_time
unlock_time = time.mktime(t)
return total_time
def run_location(self):
while serial_to_Arduino.globalvar_connection.connection.trulyConnectedAfterReceivingResponse == False:
print("telem is waiting for Arduino to connect before trying to connect")
time.sleep(1)
arduinoserport = str(serial_to_Arduino.globalvar_connection.connection.ser.port)
if arduinoserport in self.possibleSerPorts:
self.possibleSerPorts.remove(arduinoserport)
print("Since arduino connected on "+arduinoserport+", will try to connect telem on other ports: "+str(self.possibleSerPorts))
keeptrying = True
while keeptrying:
try:
self.conn = mavutil.mavlink_connection(self.serport, baud=57600)
time.sleep(1)
keeptrying = False
print("telemetry was plugged into \'"+self.serport+"\'")
except serial.serialutil.SerialException:
print("waiting for telemetry USB to be plugged in...")
time.sleep(1)
# if we can't find it on USB0, try other USB ports
self.possibleSerPortIdxCheck += 1
if self.possibleSerPortIdxCheck == len(self.possibleSerPorts):
self.possibleSerPortIdxCheck = 0
self.serport = self.possibleSerPorts[self.possibleSerPortIdxCheck]
print("Telemetry USB was connected successfully")
self.ispluggedinlocker.acquire()
self.is_plugged_in_and_working = True
self.ispluggedinlocker.release()
print "Set CALLBACK"
self.conn.mav.set_callback(self.get_gps_callback)
print "Getting mavlink Connection:"
while True:
location = self.conn.location()
#location = mavutil.location(37.235,39.323,400,20, 90)
self.lock.acquire()
self.current = {"lat":location.lat,"lng":location.lng, "alt":location.alt, "rel_alt":location.rel_alt, "heading":location.heading}
self.lock.release()
time.sleep(0.01)
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
# open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
data = []
mag = None
offsets = Vector3(0, 0, 0)
# now gather all the data
while True:
# get the next MAVLink message in the log
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if m.get_type() == "SENSOR_OFFSETS":
# update offsets that were used during this flight
offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
if m.get_type() == "RAW_IMU" and offsets != None:
# extract one mag vector, removing the offsets that were
# used during that flight to get the raw sensor values
mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets
data.append(mag)
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
# run the fitting algorithm
ofs = offsets
ofs = Vector3(0, 0, 0)
for r in range(opts.repeat):
ofs = find_offsets(data, ofs)
print('Loop %u offsets %s' % (r, ofs))
sys.stdout.flush()
print("New offsets: %s" % ofs)
parser = OptionParser("roscopter.py [options]")
parser.add_option("--baudrate", dest="baudrate", type='int',
help="master port baud rate", default=57600)
parser.add_option("--device", dest="device", default="/dev/ttyUSB0", help="serial device")
parser.add_option("--rate", dest="rate", default=10, type='int', help="requested stream rate")
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
default=255, help='MAVLink source system for this GCS')
parser.add_option("--enable-control",dest="enable_control", default=False, help="Enable listning to control messages")
(opts, args) = parser.parse_args()
import mavutil
# create a mavlink serial instance
master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate)
if opts.device is None:
print("You must specify a serial device")
sys.exit(1)
def wait_heartbeat(m):
'''wait for a heartbeat so we know the target system IDs'''
print("Waiting for APM heartbeat")
m.wait_heartbeat()
print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system))
#This does not work yet because APM does not have it implemented
#def mav_control(data):
# '''
def write_rom():
inmaster = mavutil.mavlink_connection(indevice, input=False)
inmaster.mav.command_long_send(20, 0, mavlink.MAV_CMD_PREFLIGHT_STORAGE, 0,
1, 0, 0, 0, 0, 0, 0)
print "write_rom"
def fly_ArduPlane(viewerip=None):
'''fly ArduPlane in SIL
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time
'''
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip:
options += " --out=%s:14550" % viewerip
sil = util.start_SIL('ArduPlane', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
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/ArduPlane.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
cmd += " --home=%s --wind=%s" % (HOME_LOCATION, WIND)
if viewerip:
cmd += " --fgout=%s:5503" % viewerip
runsim = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Simulator ready to fly')
sil = util.start_SIL('ArduPlane')
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduPlane-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
