def run(self):
print("Communication worker started")
mavin = mavutil.mavlink_connection(self.udpin)
mavout = mavutil.mavlink_connection(self.udpout)
recv = None
# wait heartbeat
m = mavin.recv_match(type="HEARTBEAT", blocking=True, timeout=5)
if m is not None:
self.from_pnc.put_nowait(m)
self.gui.event_generate('<<NewParam>>', when='tail')
mavout.target_system = m.get_srcSystem()
else:
self.gui.update("Connection time is out")
return
# acquire PIDs settings
self._recv_param_all(mavin, mavout)
# main working loop
while not (self.__stop.is_set()):
recv = None
try:
recv = self.to_pnc.get_nowait()
except Empty:
pass
if recv is not None:
pass
time.sleep(0.02)
# that's all
print("Communication worker stopped")
def mavlink_thread():
# Huge try catch in case we see http://bugs.python.org/issue1856
try:
while True:
# Downtime
time.sleep(0.05)
while True:
try:
msg = inudp.get_nowait()
self.master.write(msg)
except socket.error as error:
if error.errno == ECONNABORTED:
errprinter("reestablishing connection after read timeout")
try:
self.master.close()
except:
pass
self.master = mavutil.mavlink_connection(self.master.address)
continue
# If connection reset (closed), stop polling.
return
except Empty:
break
except Exception as e:
errprinter("mav send error:", e)
break
while True:
try:
msg = self.master.recv_msg()
except socket.error as error:
if error.errno == ECONNABORTED:
errprinter("reestablishing connection after send timeout")
try:
self.master.close()
except:
pass
self.master = mavutil.mavlink_connection(self.master.address)
continue
# If connection reset (closed), stop polling.
return
except Exception as e:
# TODO debug these.
# errprinter('mav recv error:', e)
msg = None
if not msg:
break
outudp.put((msg, 6760))
except Exception as e:
# http://bugs.python.org/issue1856
if self.exiting:
pass
else:
raise e
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().startswith('DATA'):
continue
if msg.get_type() == 'PARAM_VALUE':
params.append(msg)
if not opts.HIL:
mout.write(msg.get_msgbuf())
else:
send_HIL(mlog, mout)
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 setup_processes(self, datarate, receive_url):
self.send_queue = multiprocessing.Queue()
conn = mavutil.mavlink_connection(SERIAL_PORT)
self.send_process = SendProcess(self.send_queue, conn, datarate)
self.receive_queue = multiprocessing.Queue()
receive_conn = mavutil.mavlink_connection(receive_url)
self.receive_process = ReceiveProcess(self.receive_queue, receive_conn)
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 setUp(self):
"""Creates a playback and forward of MAVLink packets."""
super(TestMavlink, self).setUp()
# Create input and output logs to simulate a source.
log_filepath = os.path.join(
os.path.dirname(__file__), "testdata/mav.tlog")
self.mlog = mavutil.mavlink_connection(log_filepath)
self.mout = mavutil.mavlink_connection('127.0.0.1:14550', input=False)
# Start the forwarding on the CLI.
self.forward = subprocess.Popen(
self.cli_base_args + ['mavlink', '--device', '127.0.0.1:14550'])
time.sleep(1) # Allow time to start forward.
def __init__(self, device, baudrate, export_host):
self.export_conn = None
self.rate = 10
if export_host:
self.export_conn = mavutil.mavlink_connection(
"udp:" + export_host, input=False)
self.conn = mavutil.mavlink_connection(device, baud=baudrate)
self.pub_attitude = rospy.Publisher('attitude', rospilot.msg.Attitude)
self.pub_rcstate = rospy.Publisher('rcstate', rospilot.msg.RCState)
self.pub_gpsraw = rospy.Publisher('gpsraw', rospilot.msg.GPSRaw)
self.pub_basic_status = rospy.Publisher('basic_status', rospilot.msg.BasicStatus)
rospy.Subscriber("set_mode", rospilot.msg.BasicMode, self.handle_set_mode)
def run(self):
master=None
dest_mav_udp=None
try:
print "Firing up PIXHAWK Handler on %s" % self.mav_port
master = mavutil.mavlink_connection(self.mav_port)
print("PIXHAWK: Waiting for heartbeat")
master.wait_heartbeat()
print("PIXHAWK: Heartbeat from APM (system %u component %u)"
% (master.target_system, master.target_system))
print("PIXHAWK: Sending all stream request for rate %u" % self.mav_streamrate)
for i in range(0, 3):
master.mav.request_data_stream_send(master.target_system, master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL, self.mav_streamrate, 1)
dest_mav_udp = mavutil.mavlink_connection('udpout:%s:%d' % (self.server_ip, self.mav_server_port))
# Hook up master to the destination.
# Any message we receive, we pipe to the destination as well.
# TODO(njoubert): we might want to filter messages first before sending them!
master.logfile_raw = dest_mav_udp
self.ready()
while not self.stopped():
msg = master.recv_match(type=self.listen_for_mav, blocking=True)
if not msg or msg is None:
pass
if msg.get_type() == 'BAD_DATA':
continue
else:
if master.packet_loss() > self.report_packet_loss_threshold:
print "PIXHAWK: Incoming packet loss exceeding %.2f%%" % self.report_packet_loss_threshold
''' We've already hooked up the sockets to each other up above'''
print "Exiting!"
except:
traceback.print_exc()
finally:
print "Shutting down..."
if master:
print "Closing Master..."
master.close()
if dest_mav_udp:
print "Closing UDP..."
dest_mav_udp.close()
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
"""fly ArduCopter in SIL for AVC2013 mission
"""
global homeloc
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
sil = util.start_SIL(binary, wipe=True, model="heli", home=home, speedup=speedup_default)
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options="--sitl=127.0.0.1:5501 --out=127.0.0.1:19550")
mavproxy.expect("Received [0-9]+ parameters")
# setup test parameters
mavproxy.send("param load %s/Helicopter.parm\n" % testdir)
mavproxy.expect("Loaded [0-9]+ parameters")
mavproxy.send("param set LOG_REPLAY 1\n")
mavproxy.send("param set LOG_DISARMED 1\n")
time.sleep(3)
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model="heli", home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
options = "--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5"
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
options += " --map"
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options=options)
mavproxy.expect("Telemetry log: (\S+)")
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# 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, expect_callback)
expect_list_clear()
expect_list_extend([sil, mavproxy])
if map:
mavproxy.send("map icon 40.072467969730496 -105.2314389590174\n")
mavproxy.send("map icon 40.072600990533829 -105.23146100342274\n")
# 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 scan(self, filename):
'''scan a tlog file for positions'''
mlog = mavutil.mavlink_connection(filename, robust_parsing=True)
idx = 0
print("Scanning %u images" % len(self.images))
while idx < len(self.images):
msg = mlog.recv_match(type='GLOBAL_POSITION_INT')
if msg is None:
break
bearing = msg.hdg*0.01
speed = math.sqrt(msg.vx**2 + msg.vy**2)
while idx < len(self.images) and msg._timestamp > self.images[idx].frame_time:
dt = msg._timestamp - self.images[idx].frame_time
pos = (msg.lat*1.0e-7, msg.lon*1.0e-7)
pos = cuav_util.gps_newpos(pos[0], pos[1], bearing, speed*dt)
self.images[idx].pos = pos
latint = int(pos[0]*1000)
lonint = int(pos[1]*1000)
if not latint in self.latset:
self.latset[latint] = set()
if not lonint in self.lonset:
self.lonset[lonint] = set()
self.latset[latint].add(idx)
self.lonset[lonint].add(idx)
idx += 1
print("Scanned %u images" % idx)
def process_tlog(filename):
'''convert a tlog to a .m file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, dialect=opts.dialect)
# first walk the entire file, grabbing all messages into a list, and the last message of each type
# into a hash
msg_types = {}
msg_list = []
types = opts.types
if types is not None:
types = types.split(',')
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if types is not None and m.get_type() not in types:
continue
if m.get_type() == 'BAD_DATA':
continue
msg_list.append(m)
msg_types[m.get_type()] = m
# note that Octave doesn't like any extra '.' characters in the filename
basename = '.'.join(filename.split('.')[:-1])
mfilename = basename.replace('.','_') + '.m'
print("Creating %s" % mfilename)
create_mfile(mfilename, msg_types, msg_list)
def connect(self):
try:
print("Connecting to autopilot...")
self.mav = mavutil.mavlink_connection(self.host, use_native=False)
print("Connected to autopilot!")
print("Waiting for heartbeat...")
self.mav.wait_heartbeat(blocking=True)
print("Got Heartbeat!")
print("Waiting for plane GPS")
msg = self.mav.recv_match(type=['GLOBAL_POSITION_INT'],blocking=True)
print("Got plane GPS!")
self.lat = (msg.lat) / (10000000.0)
self.lon = (msg.lon) / (10000000.0)
self.alt = msg.alt
self.relative_alt = (msg.relative_alt) / (1000.0)
self.heading = (msg.hdg) / (100.0)
self.rally = mavwp.MAVRallyLoader(self.mav.target_system,self.mav.target_component)
self.rally.create_and_append_rally_point(self.lat * 1e7,self.lon * 1e7 ,100,50,0,0)
self.rallypoint = self.rally.rally_point(0)
self.rallypoint.target_system = self.mav.target_system
self.rallypoint.target_component = self.mav.target_component
self.mav.mav.send(self.rallypoint)
return True
except:
return False
def run(self):
try:
self.master = mavutil.mavlink_connection('udpin:%s:%d' % (self.bind_ip, self.mav_port))
print "Module %s listening on %s" % (self, self.mav_port)
self.ready()
while not self.stopped():
msg = self.master.recv_match(type=self.listen_for_mav, blocking=True)
if not msg or msg is None:
pass
if msg.get_type() == 'BAD_DATA':
continue
else:
self.handle_incoming(msg)
if self.master.packet_loss() > self.report_packet_loss_threshold:
print "PIXHAWK UDP: Incoming packet loss exceeding %.2f%%" % self.report_packet_loss_threshold
except SystemExit:
pass
except:
traceback.print_exc()
finally:
if self.master:
self.master.close()
def fly_CopterAVC(viewerip=None, map=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
global homeloc
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
sil = util.start_SIL('ArduCopter', wipe=True, model='heli', home=home, speedup=speedup_default)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/Helicopter.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', model='heli', home=home, speedup=speedup_default)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# 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, expect_callback)
expect_list_clear()
expect_list_extend([sil, mavproxy])
if map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# 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 link_add(self, device):
'''add new link'''
try:
print("Connect %s source_system=%d" % (device, self.settings.source_system))
conn = mavutil.mavlink_connection(device, autoreconnect=True,
source_system=self.settings.source_system,
baud=self.settings.baudrate)
conn.mav.srcComponent = self.settings.source_component
except Exception as msg:
print("Failed to connect to %s : %s" % (device, msg))
return False
if self.settings.rtscts:
conn.set_rtscts(True)
conn.linknum = len(self.mpstate.mav_master)
conn.mav.set_callback(self.master_callback, conn)
if hasattr(conn.mav, 'set_send_callback'):
conn.mav.set_send_callback(self.master_send_callback, conn)
conn.linknum = len(self.mpstate.mav_master)
conn.linkerror = False
conn.link_delayed = False
conn.last_heartbeat = 0
conn.last_message = 0
conn.highest_msec = 0
self.mpstate.mav_master.append(conn)
self.status.counters['MasterIn'].append(0)
try:
mp_util.child_fd_list_add(conn.port.fileno())
except Exception:
pass
return True
def on_click_connect(self,e):
"""
Process a click on the CONNECT button
Attempt to connect to the MAV using the specified port and baud rate,
then subscribe a function called check_heartbeat that will listen for
a heartbeat message, as well as a function that will print all incoming
MAVLink messages to the console.
"""
port = self.cb_port.GetValue()
baud = int(self.cb_baud.GetValue())
self.textOutput.AppendText("Connecting to " + port + " at " + str(baud) + " baud\n")
self.master = mavutil.mavlink_connection(port, baud=baud)
self.thread = threading.Thread(target=self.process_messages)
self.thread.setDaemon(True)
self.thread.start()
self.master.message_hooks.append(self.check_heartbeat)
self.master.message_hooks.append(self.log_message)
print("Connecting to " + port + " at " + str(baud) + "baud")
self.textOutput.AppendText("Waiting for APM heartbeat\n")
return
def mavloss(logfile):
'''work out signal loss times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename,
planner_format=args.planner,
notimestamps=args.notimestamps,
robust_parsing=args.robust)
# Track the reasons for MAVLink parsing errors and print them all out at the end.
reason_ids = set()
reasons = []
while True:
m = mlog.recv_match(condition=args.condition)
# Stop parsing the file once we've reached the end
if m is None:
break
# Save the parsing failure reason for this message if it's a new one
if m.get_type() == 'BAD_DATA':
reason_id = ''.join(m.reason.split(' ')[0:3])
if reason_id not in reason_ids:
reason_ids.add(reason_id)
reasons.append(m.reason)
# Print out the final packet loss results
print("%u packets, %u lost %.1f%%" % (
mlog.mav_count, mlog.mav_loss, mlog.packet_loss()))
# Also print out the reasons why losses occurred
if len(reasons) > 0:
print("Packet loss at least partially attributed to the following:")
for r in reasons:
print(" * " + r)
def __init__(self, master, sysID=1, compoID=250):
if master == "/dev/ttyACM0":
master = detect_pixhawk()
print "Pixhawk Master is", master
baud = 115200
reboot = False
else:
baud = 57600
reboot = True
self.conn = mavutil.mavlink_connection(device=master, baud=baud,
autoreconnect=True)
if reboot:
logging.info("Reboot pixhawk !")
self.conn.reboot_autopilot()
time.sleep(0.5)
self.sysID = sysID
self.compoID = compoID
self.heartbeat_period = mavutil.periodic_event(1)
self.heartbeat_period.frequency = 1.0
self.recv_msg_count = 0
self.bad_data_count = 0
self.param_names = {}
for i in range(1, 8):
self.param_names["RC%d_MIN" % i] = None
self.param_names["RC%d_MAX" % i] = None
self.initialize()
def connectAC(self):
'''Connects to aircraft'''
self.master = mavutil.mavlink_connection(self.ac.address)
sys.stdout.flush()
timeout = 5 #seconds
t0 = time.time()
msg = None
elapsed = 0
while time.time() - t0 <= timeout and msg == None:
msg = self.master.wait_heartbeat(blocking=False)
if time.time()-t0 >= elapsed:
print('Waiting for aircraft heartbeat...'),
print (str(elapsed)+'\r'),
sys.stdout.flush()
elapsed += 1
if msg == None:
#raise StandardError('Cannot communicate with the aircraft.')
print ('Waiting for aircraft heartbeat...failure')
self.connect_status = 'Not connected'
else:
print 'Waiting for aircraft heartbeat...success'
self.connected = True
self.connect_status = 'Connected'
time.sleep(1)
def process_tlog(filename):
'''convert a tlog to a .m file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, dialect=opts.dialect, zero_time_base=True)
# first walk the entire file, grabbing all messages into a hash of lists,
#and the first message of each type into a hash
msg_types = {}
msg_lists = {}
types = opts.types
if types is not None:
types = types.split(',')
# note that Octave doesn't like any extra '.', '*', '-', characters in the filename
(head, tail) = os.path.split(filename)
basename = '.'.join(tail.split('.')[:-1])
mfilename = re.sub('[\.\-\+\*]','_', basename) + '.m'
if head is not None:
mfilename = os.path.join(head, mfilename)
print("Creating %s" % mfilename)
f = open(mfilename, "w")
type_counters = {}
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if types is not None and m.get_type() not in types:
continue
if m.get_type() == 'BAD_DATA':
continue
fieldnames = m._fieldnames
mtype = m.get_type()
if mtype in ['FMT', 'PARM']:
continue
if mtype not in type_counters:
type_counters[mtype] = 0
f.write("%s.heading = {'timestamp'" % mtype)
for field in fieldnames:
val = getattr(m, field)
if not isinstance(val, str):
f.write(",'%s'" % field)
f.write("};\n")
type_counters[mtype] += 1
f.write("%s.data(%u,:) = [%f" % (mtype, type_counters[mtype], m._timestamp))
for field in m._fieldnames:
val = getattr(m, field)
if not isinstance(val, str):
f.write(",%f" % val)
f.write("];\n")
f.close()
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
m = mlog.recv_match(type=['MISSION_ITEM','CMD','WAYPOINT'])
if m is None:
break
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
wp.save(args.output)
print("Saved %u waypoints to %s" % (wp.count(), args.output))
def magfit(logfile):
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
cal = Calibrator()
offsets = Vector3(0,0,0)
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
cal.add_sample(mag - offsets + noise())
if m.get_type() == "MAG":
offsets = Vector3(m.OfsX, m.OfsY, m.OfsZ)
mag = Vector3(m.MagX, m.MagY, m.MagZ)
cal.add_sample(mag - offsets + noise())
(offsets, scaling) = cal.calibrate()
print len(cal.samples)
print offsets, scaling
graph(cal.samples, offsets, scaling)
print "mavgraph.py %s mag_field(RAW_IMU,SENSOR_OFFSETS,(%f,%f,%f),(%f,%f,%f)" % (filename,offsets.x,offsets.y,offsets.z,scaling.x,scaling.y,scaling.z)
def process_file(filename, source):
'''process one file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
position_field_type = sniff_field_spelling(mlog, source)
# init fields and field_types lists
fields = [args.source + "." + s for s in position_field_type]
fields.append(mainstate_field)
field_types = []
msg_types = set()
re_caps = re.compile('[A-Z_][A-Z0-9_]+')
for f in fields:
caps = set(re.findall(re_caps, f))
msg_types = msg_types.union(caps)
field_types.append(caps)
add_data.new_linestring = True
add_data.mainstate_current = -1
add_data.current_kml_linestring = None
add_data.position_data = [None for n in position_field_type]
add_data.last_time = 0
while True:
msg = mlog.recv_match(args.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, msg_types, mlog.messages, fields, field_types, position_field_type)
def _initialize(self, sync=False):
# Try to close any old connection first
if self._master is not None:
try:
self._master.close()
except:
pass
# Cycle until device becomes available and we can connect to it
device_not_found_once = False
while not rospy.is_shutdown():
device = self._device
# If not a network connection, check for matching file
if device is None or ':' not in device:
device = self._find_device(device)
# If no device found
if device is None:
# Display a warning (first time)
if not device_not_found_once:
rospy.logwarn("MAVLinkBridge: no device found, " + \
"will keep checking ...")
device_not_found_once = True
# Wait a moment, then try again
time.sleep(1)
continue
# Try to create connection
try:
rospy.loginfo("MAVLinkBridge: Connecting to %s ..." % device)
self._master = mavutil.mavlink_connection(
device,
int(self._baudrate),
autoreconnect=True)
except Exception as ex:
rospy.logwarn("MAVLinkBridge: " + str(ex))
# If succeeded, move on to rest of init
break
# If failed, wait a moment then try again
time.sleep(1)
# Wait for a heartbeat so we know the target system IDs
rospy.loginfo("MAVLinkBridge: Waiting for autopilot heartbeat ...")
self._master.wait_heartbeat()
# Set up output stream from master
self._master.mav.request_data_stream_send(
self._master.target_system,
self._master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL,
self._mavlink_rate,
1)
# Initialize clock reference
self._init_time(sync)
rospy.loginfo("MAVLinkBridge initialized.")
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','GPS'], 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
if m.get_type() == 'VFR_HUD':
groundspeed = m.groundspeed
else:
groundspeed = m.Spd
t = time.localtime(m._timestamp)
if groundspeed > opts.groundspeed and not in_air:
print("In air at %s (percent %.0f%% groundspeed %.1f)" % (time.asctime(t), mlog.percent, groundspeed))
in_air = True
start_time = time.mktime(t)
elif groundspeed < opts.groundspeed and in_air:
print("On ground at %s (percent %.1f%% groundspeed %.1f time=%.1f seconds)" % (
time.asctime(t), mlog.percent, groundspeed, time.mktime(t) - start_time))
in_air = False
total_time += time.mktime(t) - start_time
return total_time
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 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.tlog" % (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 mavparms(logfile):
'''extract mavlink parameters'''
mlog = mavutil.mavlink_connection(filename)
while True:
try:
m = mlog.recv_match(type=['PARAM_VALUE', 'PARM'])
if m is None:
return
except Exception:
return
if m.get_type() == 'PARAM_VALUE':
pname = str(m.param_id).strip()
value = m.param_value
else:
pname = m.Name
value = m.Value
if len(pname) > 0:
if args.changesOnly is True and pname in parms and parms[
pname] != value:
print(
(
"%s %-15s %.6f -> %.6f" %
(time.asctime(
time.localtime(
m._timestamp)),
pname,
parms[pname],
value)))
parms[pname] = value
def drive_APMrover2(viewerip=None, map=False, valgrind=False):
'''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
if map:
options += ' --map'
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sil = util.start_SIL('APMrover2', wipe=True, model='rover', home=home, speedup=10)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
print("WAITING FOR PARAMETERS")
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
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)
sil = util.start_SIL('APMrover2', model='rover', home=home, speedup=10, valgrind=valgrind)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([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 __init__(self, port, baud= 57600, name=id_gen): self.name = name self.master = mavutil.mavlink_connection(device=port, baud=baud, autoreconnect=False) self.lat = None self.lon = None self.alt = None self.thread = threading.Thread(target = self.update) self.thread.start()
import time
import MAVProxy
from pymavlink import mavwp
from pymavlink import mavutil
from MAVProxy.modules.lib import mp_module
from MAVProxy.modules.lib import mp_util
#from MAVProxy import mavproxy_wp
from MAVProxy.modules import mavproxy_wp
#to establish connection with sitl
mav1 = mavutil.mavlink_connection('udp:127.0.0.1:14550')
mav1.wait_heartbeat()
#load waypoints from a txt file
wp = mavwp.MAVWPLoader()
wp.load("abc.txt")
#wp._read_waypoints_v110("abc.txt")
#print the no. of waypoints
print "%d" % wp.count()
#print waypoint info of 2
print "%s" % wp.wp(2)
mav1.waypoint_clear_all_send()
mav1.waypoint_count_send(wp.count())
for i in range(wp.count()):
msg = mav1.recv_match(type=['MISSION_REQUEST'], blocking=True)
mav1.mav.send(wp.wp(msg.seq))
print 'Sending waypoint {0}'.format(msg.seq)
parser.add_option("--types",
default=None,
help="types of messages (comma separated)")
parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect")
(opts, args) = parser.parse_args()
from pymavlink import mavutil
if len(args) < 1:
print("Usage: mavlogdump.py [options] <LOGFILE>")
sys.exit(1)
filename = args[0]
mlog = mavutil.mavlink_connection(filename,
planner_format=opts.planner,
notimestamps=opts.notimestamps,
robust_parsing=opts.robust,
dialect=opts.dialect)
output = None
if opts.output:
output = mavutil.mavlogfile(opts.output, write=True)
types = opts.types
if types is not None:
types = types.split(',')
while True:
m = mlog.recv_match(condition=opts.condition, blocking=opts.follow)
if m is None:
break
def main():
parser = OptionParser("spab.py [options]")
parser.add_option("--baudrate",
dest="baudrate",
type='int',
help='master port baud rate',
default=57600)
parser.add_option("--device",
dest="device",
default=None,
help="serial device")
parser.add_option("--modem",
dest="mport",
default=None,
help="modem serial port")
parser.add_option("--rate",
dest="rate",
default=4,
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("--showmessages",
dest="showmessages",
action='store_true',
help="show incoming messages",
default=False)
parser.add_option("--logfile",
dest="log",
default=None,
help="name of logfile")
(opts, args) = parser.parse_args()
if opts.device is None or opts.mport is None:
print("You must specify a mavlink device and a modem device")
sys.exit(1)
signal.signal(signal.SIGINT, catch)
# init objects
try:
master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate)
modem = F2414Modem.F2414Modem(opts.mport, opts.baudrate)
spabModel = SpabModel.SpabModel()
telemManager = TelemManager.TelemManager(task, spabModel, modem,
telemPeriod)
mavlinkManager = MavlinkManager.MavlinkManager(task, spabModel,
telemPeriod, master)
except:
print('failed to find devices')
sys.exit(1)
if opts.log is not None:
logFile = open(opts.log, "a")
sys.stderr = logFile
sys.stdout = logFile
print("====PYSPAB====\r")
# init delegates
global Delegates
Delegates = {
"HEARTBEAT": mavlinkManager.handle_heartbeat,
"VFR_HUD": mavlinkManager.handle_hud,
"ATTITUDE": mavlinkManager.handle_attitude,
"GLOBAL_POSITION_INT": mavlinkManager.handle_gps_filtered,
"MISSION_REQUEST": mavlinkManager.handle_mission_request,
"MISSION_ACK": mavlinkManager.handle_mission_ack,
"RC_CHANNELS_RAW": mavlinkManager.handle_rc_raw,
"BAD_DATA": mavlinkManager.handle_bad_data,
"MISSION_COUNT": mavlinkManager.handle_mission_count,
"MISSION_ITEM": mavlinkManager.handle_mission_item
#"MISSION_CURRENT": mavlinkManager.handle_mission_current
}
# set to run
master.wait_heartbeat()
master.mav.request_data_stream_send(master.target_system,
master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL,
opts.rate, 1)
master.mav.set_mode_send(master.target_system, 216, 216)
telemManager.start()
mavlinkManager.start()
task.run(False)
read_loop(master)
def process(filename):
'''process one logfile'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps,
robust_parsing=args.robust)
ext = os.path.splitext(filename)[1]
isbin = ext in ['.bin', '.BIN']
islog = ext in ['.log', '.LOG']
output = None
count = 1
dirname = os.path.dirname(filename)
if isbin or islog:
extension = "bin"
else:
extension = "tlog"
file_header = bytearray()
messages = []
# we allow a list of modes that map to one mode number. This allows for --mode=AUTO,RTL and consider the RTL as part of AUTO
modes = args.mode.upper().split(',')
flightmode = None
while True:
m = mlog.recv_match()
if m is None:
break
if args.link is not None and m._link != args.link:
continue
mtype = m.get_type()
if mtype in messages:
if older_message(m, messages[mtype]):
continue
# we don't use mlog.flightmode as that can be wrong if we are extracting a single link
if mtype == 'HEARTBEAT' and m.get_srcComponent() != mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.type != mavutil.mavlink.MAV_TYPE_GCS:
flightmode = mavutil.mode_string_v10(m).upper()
if mtype == 'MODE':
flightmode = mlog.flightmode
if (isbin or islog) and m.get_type() in ["FMT", "PARM", "CMD", "FMTU", "MULT"]:
file_header += m.get_msgbuf()
if (isbin or islog) and m.get_type() == 'MSG' and m.Message.startswith("Ardu"):
file_header += m.get_msgbuf()
if m.get_type() in ['PARAM_VALUE','MISSION_ITEM','MISSION_ITEM_INT']:
timestamp = getattr(m, '_timestamp', None)
file_header += struct.pack('>Q', int(timestamp*1.0e6)) + m.get_msgbuf()
if not mavutil.evaluate_condition(args.condition, mlog.messages):
continue
if flightmode in modes:
if output is None:
path = os.path.join(dirname, "%s%u.%s" % (modes[0], count, extension))
count += 1
print("Creating %s" % path)
output = open(path, mode='wb')
output.write(file_header)
else:
if output is not None:
output.close()
output = None
if output and m.get_type() != 'BAD_DATA':
timestamp = getattr(m, '_timestamp', None)
if not isbin:
output.write(struct.pack('>Q', int(timestamp*1.0e6)))
output.write(m.get_msgbuf())
## Main code starts here ##
######################################################
try:
# Note: 'version' attribute is supported from pyrealsense2 2.31 onwards and might require building from source
progress("INFO: pyrealsense2 version: %s" % str(rs.__version__))
except Exception:
# fail silently
pass
progress("INFO: Starting Vehicle communications")
print(connection_string)
conn = mavutil.mavlink_connection(
device=str(connection_string),
autoreconnect=True,
source_system=1,
source_component=93,
baud=connection_baudrate,
force_connected=True,
)
mavlink_callbacks = {}
mavlink_thread = threading.Thread(target=mavlink_loop,
args=(conn, mavlink_callbacks))
mavlink_thread.start()
# connecting and configuring the camera is a little hit-and-miss.
# Start a timer and rely on a restart of the script to get it working.
# Configuring the camera appears to block all threads, so we can't do
# this internally.
# send_msg_to_gcs('Setting timer...')
signal.setitimer(signal.ITIMER_REAL, 5) # seconds...
def fly_ArduPlane(viewerip=None, map=False):
'''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=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
options += ' --map'
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.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
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 get_battery_status():
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()
battery_status = master.messages['SYS_STATUS']
cell_voltage = master.messages['BATTERY_STATUS'].voltages
return cell_voltage, battery_status
#!/usr/bin/env python3
from pymavlink import mavutil, mavwp
import time
import math
import matplotlib.pyplot as plt
from drawnow import drawnow
import utm
# Start a connection listening to a UDP port
mav = mavutil.mavlink_connection('udpin:localhost:14550')
# Wait for the first heartbeat
# This sets the system and component ID of remote system for the link
mav.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %
(mav.target_system, mav.target_system))
# Make Waypoints
wp = mavwp.MAVWPLoader()
#Get home function, lat/long are returned *10^7
def cmd_get_home():
mav.mav.command_long_send(mav.target_system, mav.target_component,
mavutil.mavlink.MAV_CMD_GET_HOME_POSITION, 0, 0,
0, 0, 0, 0, 0, 0)
msg = mav.recv_match(type=['COMMAND_ACK'], blocking=True)
print(msg)
msg = mav.recv_match(type=['HOME_POSITION'], blocking=True)
return (msg.latitude / 10000000.0, msg.longitude / 10000000.0,
msg.altitude / 10000000.0)
def fly_ArduCopter(binary,
viewerip=None,
use_map=False,
valgrind=False,
gdb=False,
frame=None,
params=None,
gdbserver=False,
speedup=10):
"""Fly ArduCopter in SITL.
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
if frame is None:
frame = '+'
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sitl = util.start_SITL(binary,
wipe=True,
model=frame,
home=home,
speedup=speedup)
mavproxy = util.start_MAVProxy_SITL(
'ArduCopter',
options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
if params is None:
params = vinfo.options["ArduCopter"]["frames"][frame][
"default_params_filename"]
if not isinstance(params, list):
params = [params]
for x in params:
mavproxy.send("param load %s\n" % os.path.join(testdir, x))
mavproxy.expect('Loaded [0-9]+ parameters')
mavproxy.send("param set LOG_REPLAY 1\n")
mavproxy.send("param set LOG_DISARMED 1\n")
time.sleep(3)
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sitl)
sitl = util.start_SITL(binary,
model=frame,
home=home,
speedup=speedup,
valgrind=valgrind,
gdb=gdb,
gdbserver=gdbserver)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if use_map:
options += ' --map'
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)\r\n')
logfile = mavproxy.match.group(1)
progress("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
progress("buildlog=%s" % buildlog)
copy_tlog = False
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
progress("WARN: Failed to create symlink: " + logfile + " => " +
buildlog + ", Will copy tlog manually to target location")
copy_tlog = True
# 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, expect_callback)
expect_list_clear()
expect_list_extend([sitl, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception as msg:
progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
failed_test_msg = "None"
try:
mav.wait_heartbeat()
setup_rc(mavproxy)
homeloc = mav.location()
wait_ready_to_arm(mav)
# Arm
progress("# Arm motors")
if not arm_motors(mavproxy, mav):
failed_test_msg = "arm_motors failed"
progress(failed_test_msg)
failed = True
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Fly a square in Stabilize mode
progress("#")
progress(
"########## Fly a square and save WPs with CH7 switch ##########")
progress("#")
if not fly_square(mavproxy, mav):
failed_test_msg = "fly_square failed"
progress(failed_test_msg)
failed = True
# save the stored mission to file
progress("# Save out the CH7 mission to file")
if not save_mission_to_file(mavproxy, mav,
os.path.join(testdir, "ch7_mission.txt")):
failed_test_msg = "save_mission_to_file failed"
progress(failed_test_msg)
failed = True
# fly the stored mission
progress("# Fly CH7 saved mission")
if not fly_mission(
mavproxy, mav, height_accuracy=0.5, target_altitude=10):
failed_test_msg = "fly ch7_mission failed"
progress(failed_test_msg)
failed = True
# Throttle Failsafe
progress("#")
progress("########## Test Failsafe ##########")
progress("#")
if not fly_throttle_failsafe(mavproxy, mav):
failed_test_msg = "fly_throttle_failsafe failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Battery failsafe
if not fly_battery_failsafe(mavproxy, mav):
failed_test_msg = "fly_battery_failsafe failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Stability patch
progress("#")
progress("########## Test Stability Patch ##########")
progress("#")
if not fly_stability_patch(mavproxy, mav, 30):
failed_test_msg = "fly_stability_patch failed"
progress(failed_test_msg)
failed = True
# RTL
progress("# RTL #")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL after stab patch failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Fence test
progress("#")
progress("########## Test Horizontal Fence ##########")
progress("#")
if not fly_fence_test(mavproxy, mav, 180):
failed_test_msg = "fly_fence_test failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Fly GPS Glitch Loiter test
progress("# GPS Glitch Loiter Test")
if not fly_gps_glitch_loiter_test(mavproxy, mav, use_map):
failed_test_msg = "fly_gps_glitch_loiter_test failed"
progress(failed_test_msg)
failed = True
# RTL after GPS Glitch Loiter test
progress("# RTL #")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL failed"
progress(failed_test_msg)
failed = True
# Fly GPS Glitch test in auto mode
progress("# GPS Glitch Auto Test")
if not fly_gps_glitch_auto_test(mavproxy, mav, use_map):
failed_test_msg = "fly_gps_glitch_auto_test failed"
progress(failed_test_msg)
failed = True
# take-off ahead of next test
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Loiter for 10 seconds
progress("#")
progress("########## Test Loiter for 10 seconds ##########")
progress("#")
if not loiter(mavproxy, mav):
failed_test_msg = "loiter failed"
progress(failed_test_msg)
failed = True
# Loiter Climb
progress("#")
progress("# Loiter - climb to 30m")
progress("#")
if not change_alt(mavproxy, mav, 30):
failed_test_msg = "change_alt climb failed"
progress(failed_test_msg)
failed = True
# Loiter Descend
progress("#")
progress("# Loiter - descend to 20m")
progress("#")
if not change_alt(mavproxy, mav, 20):
failed_test_msg = "change_alt descend failed"
progress(failed_test_msg)
failed = True
# RTL
progress("#")
progress("########## Test RTL ##########")
progress("#")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL after Loiter climb/descend failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Simple mode
progress("# Fly in SIMPLE mode")
if not fly_simple(mavproxy, mav):
failed_test_msg = "fly_simple failed"
progress(failed_test_msg)
failed = True
# RTL
progress("#")
progress("########## Test RTL ##########")
progress("#")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL after simple mode failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Fly a circle in super simple mode
progress("# Fly a circle in SUPER SIMPLE mode")
if not fly_super_simple(mavproxy, mav):
failed_test_msg = "fly_super_simple failed"
progress(failed_test_msg)
failed = True
# RTL
progress("# RTL #")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL after super simple mode failed"
progress(failed_test_msg)
failed = True
# Takeoff
progress("# Takeoff")
if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed"
progress(failed_test_msg)
failed = True
# Circle mode
progress("# Fly CIRCLE mode")
if not fly_circle(mavproxy, mav):
failed_test_msg = "fly_circle failed"
progress(failed_test_msg)
failed = True
# RTL
progress("#")
progress("########## Test RTL ##########")
progress("#")
if not fly_RTL(mavproxy, mav):
failed_test_msg = "fly_RTL after circle failed"
progress(failed_test_msg)
failed = True
progress("# Fly copter mission")
if not fly_auto_test(mavproxy, mav):
failed_test_msg = "fly_auto_test failed"
progress(failed_test_msg)
failed = True
else:
progress("Flew copter mission OK")
# wait for disarm
mav.motors_disarmed_wait()
if not log_download(mavproxy, mav,
util.reltopdir("../buildlogs/ArduCopter-log.bin")):
failed_test_msg = "log_download failed"
progress(failed_test_msg)
failed = True
except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout"
failed = True
mav.close()
util.pexpect_close(mavproxy)
util.pexpect_close(sitl)
valgrind_log = util.valgrind_log_filepath(binary=binary, model='+')
if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0o644)
shutil.copy(valgrind_log,
util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
# [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link
# This flag tells me that I need to copy the data out
if copy_tlog:
shutil.copy(logfile, buildlog)
if failed:
progress("FAILED: %s" % failed_test_msg)
return False
return True
def mavfft_fttd(logfile):
'''display fft for raw ACC data in logfile'''
'''object to store data about a single FFT plot'''
class PlotData(object):
def __init__(self, ffth):
self.seqno = -1
self.fftnum = ffth.N
self.sensor_type = ffth.type
self.instance = ffth.instance
self.sample_rate_hz = ffth.smp_rate
self.multiplier = ffth.mul
self.data = {}
self.data["X"] = []
self.data["Y"] = []
self.data["Z"] = []
self.holes = False
self.freq = None
def add_fftd(self, fftd):
if fftd.N != self.fftnum:
print("Skipping ISBD with wrong fftnum (%u vs %u)\n" %
(fftd.fftnum, self.fftnum))
return
if self.holes:
print("Skipping ISBD(%u) for ISBH(%u) with holes in it" %
(fftd.seqno, self.fftnum))
return
if fftd.seqno != self.seqno + 1:
print("ISBH(%u) has holes in it" % fftd.N)
self.holes = True
return
self.seqno += 1
self.data["X"].extend(fftd.x)
self.data["Y"].extend(fftd.y)
self.data["Z"].extend(fftd.z)
def overlap_windows(self, plotdata):
newplotdata = copy.deepcopy(self)
if plotdata.tag() != self.tag():
print("Invalid FFT data tag (%s vs %s) for window overlap" %
(plotdata.tag(), self.tag()))
return self
if plotdata.fftnum <= self.fftnum:
print("Invalid FFT sequence (%u vs %u) for window overlap" %
(plotdata.fftnum, self.fftnum))
return self
newplotdata.data["X"] = numpy.split(numpy.asarray(
self.data["X"]), 2)[1].tolist() + numpy.split(
numpy.asarray(plotdata.data["X"]), 2)[0].tolist()
newplotdata.data["Y"] = numpy.split(numpy.asarray(
self.data["Y"]), 2)[1].tolist() + numpy.split(
numpy.asarray(plotdata.data["Y"]), 2)[0].tolist()
newplotdata.data["Z"] = numpy.split(numpy.asarray(
self.data["Z"]), 2)[1].tolist() + numpy.split(
numpy.asarray(plotdata.data["Z"]), 2)[0].tolist()
return newplotdata
def prefix(self):
if self.sensor_type == 0:
return "Accel"
elif self.sensor_type == 1:
return "Gyro"
else:
return "?Unknown Sensor Type?"
def tag(self):
return str(self)
def __str__(self):
return "%s[%u]" % (self.prefix(), self.instance)
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
# see https://holometer.fnal.gov/GH_FFT.pdf for a description of the techniques used here
things_to_plot = []
plotdata = None
prev_plotdata = {}
msgcount = 0
hntch_mode = None
hntch_option = None
batch_mode = None
start_time = time.time()
while True:
m = mlog.recv_match(condition=args.condition)
if m is None:
break
msgcount += 1
if msgcount % 1000 == 0:
sys.stderr.write(".")
msg_type = m.get_type()
if msg_type == "ISBH":
if plotdata is not None:
sensor = plotdata.tag()
# close off previous data collection
# in order to get 50% window overlap we need half the old data + half the new data and then the new data itself
if args.fft_overlap and sensor in prev_plotdata:
things_to_plot.append(
prev_plotdata[sensor].overlap_windows(plotdata))
things_to_plot.append(plotdata)
prev_plotdata[sensor] = plotdata
plotdata = PlotData(m)
continue
if msg_type == "ISBD":
if plotdata is None:
sys.stderr.write("?(fftnum=%u)" % m.N)
continue
plotdata.add_fftd(m)
if msg_type == "PARM":
if m.Name == "INS_HNTCH_MODE":
hntch_mode = m.Value
elif m.Name == "INS_HNTCH_OPTS":
hntch_option = m.Value
elif m.Name == "INS_LOG_BAT_OPT":
batch_mode = m.Value
print("", file=sys.stderr)
time_delta = time.time() - start_time
print("%us messages %u messages/second %u kB/second" %
(msgcount, msgcount / time_delta,
os.stat(filename).st_size / time_delta))
print("Extracted %u fft data sets" % len(things_to_plot), file=sys.stderr)
sum_fft = {}
freqmap = {}
sample_rates = {}
counts = {}
window = {}
S2 = {}
hntch_mode_names = {0: "No", 1: "Throttle", 2: "RPM", 3: "ESC", 4: "FFT"}
hntch_option_names = {0: "Single", 1: "Double"}
batch_mode_names = {0: "Pre-filter", 1: "Sensor-rate", 2: "Post-filter"}
first_freq = None
for thing_to_plot in things_to_plot:
fft_len = len(thing_to_plot.data["X"])
if thing_to_plot.tag() not in sum_fft:
sum_fft[thing_to_plot.tag()] = {
"X": numpy.zeros(fft_len // 2 + 1),
"Y": numpy.zeros(fft_len // 2 + 1),
"Z": numpy.zeros(fft_len // 2 + 1),
}
counts[thing_to_plot.tag()] = 0
if thing_to_plot.tag() not in window:
if args.fft_window == 'hanning':
# create hanning window
window[thing_to_plot.tag()] = numpy.hanning(fft_len)
elif args.fft_window == 'blackman':
# create blackman window
window[thing_to_plot.tag()] = numpy.blackman(fft_len)
else:
window[thing_to_plot.tag()] = numpy.arange(fft_len)
window[thing_to_plot.tag()].fill(1)
# calculate NEBW constant
S2[thing_to_plot.tag()] = numpy.inner(window[thing_to_plot.tag()],
window[thing_to_plot.tag()])
for axis in ["X", "Y", "Z"]:
# normalize data and convert to dps in order to produce more meaningful magnitudes
if thing_to_plot.sensor_type == 1:
d = numpy.array(numpy.degrees(
thing_to_plot.data[axis])) / float(
thing_to_plot.multiplier)
else:
d = numpy.array(thing_to_plot.data[axis]) / float(
thing_to_plot.multiplier)
if len(d) == 0:
print("No data?!?!?!")
continue
if len(window[thing_to_plot.tag()]) != fft_len:
print("Skipping corrupted frame of length %d" % fft_len)
continue
# apply window to the input
d *= window[thing_to_plot.tag()]
# perform RFFT
d_fft = numpy.fft.rfft(d)
# convert to squared complex magnitude
d_fft = numpy.square(abs(d_fft))
# remove DC component
d_fft[0] = 0
d_fft[-1] = 0
# accumulate the sums
sum_fft[thing_to_plot.tag()][axis] += d_fft
freq = numpy.fft.rfftfreq(len(d),
1.0 / thing_to_plot.sample_rate_hz)
freqmap[thing_to_plot.tag()] = freq
sample_rates[thing_to_plot.tag()] = thing_to_plot.sample_rate_hz
counts[thing_to_plot.tag()] += 1
numpy.seterr(divide='ignore')
for sensor in sum_fft:
print("Sensor: %s" % str(sensor))
fig = pylab.figure(str(sensor))
for axis in ["X", "Y", "Z"]:
# normalize output to averaged PSD
psd = 2 * (sum_fft[sensor][axis] /
counts[sensor]) / (sample_rates[sensor] * S2[sensor])
# linerize of requested
if args.fft_output == 'lsd':
psd = numpy.sqrt(psd)
# convert to db if requested
if args.fft_scale == 'db':
psd = 10 * numpy.log10(psd)
pylab.plot(freqmap[sensor], psd, label=axis)
pylab.legend(loc='upper right')
pylab.xlabel('Hz')
scale_label = ''
psd_label = 'PSD'
if args.fft_scale == 'db':
scale_label = "dB "
if args.fft_output == 'lsd':
if str(sensor).startswith("Gyro"):
pylab.ylabel('LSD $' + scale_label + 'd/s/\sqrt{Hz}$')
else:
pylab.ylabel('LSD $' + scale_label + 'm/s^2/\sqrt{Hz}$')
else:
if str(sensor).startswith("Gyro"):
pylab.ylabel('PSD $' + scale_label + 'd^2/s^2/Hz$')
else:
pylab.ylabel('PSD $' + scale_label + 'm^2/s^4/Hz$')
if hntch_mode is not None and hntch_option is not None and batch_mode is not None:
textstr = '\n'.join(
(r'%s tracking' % (hntch_mode_names[hntch_mode], ),
r'%s notch' % (hntch_option_names[hntch_option], ),
r'%s sampling' % (batch_mode_names[batch_mode], )))
props = dict(boxstyle='round', facecolor='wheat', alpha=0.5)
pylab.text(0.5,
0.95,
textstr,
fontsize=12,
verticalalignment='top',
bbox=props,
transform=pylab.gca().transAxes)
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
global earth_field, declination
global data
data = []
ATT = None
BAT = None
mag_msg = 'MAG%s' % mag_idx
count = 0
parameters = {}
# get parameters
while True:
msg = mlog.recv_match(type=['PARM'])
if msg is None:
break
parameters[msg.Name] = msg.Value
mlog.rewind()
# extract MAG data
while True:
msg = mlog.recv_match(
type=['GPS', mag_msg, 'ATT', 'CTUN', 'BARO', 'BAT'],
condition=args.condition)
if msg is None:
break
if msg.get_type() == 'GPS' and msg.Status >= 3 and earth_field is None:
earth_field = mavextra.expected_earth_field(msg)
(declination, inclination,
intensity) = mavextra.get_mag_field_ef(msg.Lat, msg.Lng)
print("Earth field: %s strength %.0f declination %.1f degrees" %
(earth_field, earth_field.length(), declination))
if msg.get_type() == 'ATT':
ATT = msg
if msg.get_type() == 'BAT':
BAT = msg
if msg.get_type() == mag_msg and ATT is not None:
if count % args.reduce == 0:
data.append((msg, ATT, BAT))
count += 1
old_corrections.offsets = Vector3(
parameters.get('COMPASS_OFS%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_OFS%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_OFS%s_Z' % mag_idx, 0.0))
old_corrections.diag = Vector3(
parameters.get('COMPASS_DIA%s_X' % mag_idx, 1.0),
parameters.get('COMPASS_DIA%s_Y' % mag_idx, 1.0),
parameters.get('COMPASS_DIA%s_Z' % mag_idx, 1.0))
old_corrections.offdiag = Vector3(
parameters.get('COMPASS_ODI%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_ODI%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_ODI%s_Z' % mag_idx, 0.0))
if parameters.get('COMPASS_MOTCT', 0) == 2:
# only support current based corrections for now
old_corrections.cmot = Vector3(
parameters.get('COMPASS_MOT%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_MOT%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_MOT%s_Z' % mag_idx, 0.0))
old_corrections.scaling = parameters.get('COMPASS_SCALE%s' % mag_idx, None)
if old_corrections.scaling is None:
force_scale = False
old_corrections.scaling = 1.0
else:
force_scale = True
# remove existing corrections
data2 = []
for (MAG, ATT, BAT) in data:
if remove_offsets(MAG, BAT, old_corrections):
data2.append((MAG, ATT, BAT))
data = data2
print("Extracted %u points" % len(data))
print("Current: %s diag: %s offdiag: %s cmot: %s scale: %.2f" %
(old_corrections.offsets, old_corrections.diag,
old_corrections.offdiag, old_corrections.cmot,
old_corrections.scaling))
# do fit
c = fit_WWW()
# normalise diagonals to scale factor
if force_scale:
avgdiag = (c.diag.x + c.diag.y + c.diag.z) / 3.0
c.scaling *= avgdiag
c.diag *= 1.0 / avgdiag
c.offdiag *= 1.0 / avgdiag
print("New: %s diag: %s offdiag: %s cmot: %s scale: %.2f" %
(c.offsets, c.diag, c.offdiag, c.cmot, c.scaling))
if not args.plot:
return
x = []
corrected = {}
corrected['Yaw'] = []
expected1 = {}
expected2 = {}
uncorrected = {}
uncorrected['Yaw'] = []
yaw_change1 = []
yaw_change2 = []
for i in range(len(data)):
(MAG, ATT, BAT) = data[i]
yaw1 = get_yaw(ATT, MAG, BAT, c)
corrected['Yaw'].append(yaw1)
ef1 = expected_field(ATT, yaw1)
cf = correct(MAG, BAT, c)
yaw2 = get_yaw(ATT, MAG, BAT, old_corrections)
ef2 = expected_field(ATT, yaw2)
uncorrected['Yaw'].append(yaw2)
uf = correct(MAG, BAT, old_corrections)
yaw_change1.append(mavextra.wrap_180(yaw1 - yaw2))
yaw_change2.append(mavextra.wrap_180(yaw1 - ATT.Yaw))
for axis in ['x', 'y', 'z']:
if not axis in corrected:
corrected[axis] = []
uncorrected[axis] = []
expected1[axis] = []
expected2[axis] = []
corrected[axis].append(getattr(cf, axis))
uncorrected[axis].append(getattr(uf, axis))
expected1[axis].append(getattr(ef1, axis))
expected2[axis].append(getattr(ef2, axis))
x.append(i)
c.show_parms()
fig, axs = pyplot.subplots(3, 1, sharex=True)
for axis in ['x', 'y', 'z']:
axs[0].plot(numpy.array(x),
numpy.array(uncorrected[axis]),
label='Uncorrected %s' % axis.upper())
axs[0].plot(numpy.array(x),
numpy.array(expected2[axis]),
label='Expected %s' % axis.upper())
axs[0].legend(loc='upper left')
axs[0].set_title('Original')
axs[0].set_ylabel('Field (mGauss)')
axs[1].plot(numpy.array(x),
numpy.array(corrected[axis]),
label='Corrected %s' % axis.upper())
axs[1].plot(numpy.array(x),
numpy.array(expected1[axis]),
label='Expected %s' % axis.upper())
axs[1].legend(loc='upper left')
axs[1].set_title('Corrected')
axs[1].set_ylabel('Field (mGauss)')
# show change in yaw estimate from old corrections to new
axs[2].plot(numpy.array(x),
numpy.array(yaw_change1),
label='Mag Yaw Change')
axs[2].plot(numpy.array(x),
numpy.array(yaw_change2),
label='ATT Yaw Change')
axs[2].set_title('Yaw Change (degrees)')
axs[2].legend(loc='upper left')
pyplot.show()
def process_tlog(filename):
'''convert a tlog to a .m file'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, dialect=args.dialect, zero_time_base=True)
# first walk the entire file, grabbing all messages into a hash of lists,
#and the first message of each type into a hash
msg_types = {}
msg_lists = {}
types = args.types
if types is not None:
types = types.split(',')
# note that Octave doesn't like any extra '.', '*', '-', characters in the filename
(head, tail) = os.path.split(filename)
basename = '.'.join(tail.split('.')[:-1])
mfilename = re.sub('[\.\-\+\*]','_', basename) + '.m'
# Octave also doesn't like files that don't start with a letter
if re.match('^[a-zA-z]', mfilename) is None:
mfilename = 'm_' + mfilename
if head is not None:
mfilename = os.path.join(head, mfilename)
print("Creating %s" % mfilename)
f = open(mfilename, "w")
type_counters = {}
while True:
m = mlog.recv_match(condition=args.condition)
if m is None:
break
if types is not None and m.get_type() not in types:
continue
if m.get_type() == 'BAD_DATA':
continue
fieldnames = m._fieldnames
mtype = m.get_type()
if mtype in ['FMT', 'PARM']:
continue
if mtype not in type_counters:
type_counters[mtype] = 0
f.write("%s.columns = {'timestamp'" % mtype)
for field in fieldnames:
val = getattr(m, field)
if not isinstance(val, str):
if type(val) is not list:
f.write(",'%s'" % field)
else:
for i in range(0, len(val)):
f.write(",'%s%d'" % (field, i + 1))
f.write("};\n")
type_counters[mtype] += 1
f.write("%s.data(%u,:) = [%f" % (mtype, type_counters[mtype], m._timestamp))
for field in m._fieldnames:
val = getattr(m, field)
if not isinstance(val, str):
if type(val) is not list:
f.write(",%.20g" % val)
else:
for i in range(0, len(val)):
f.write(",%.20g" % val[i])
f.write("];\n")
f.close()
# Import mavutil
from pymavlink import mavutil
import time
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:15000')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# Arm
# master.arducopter_arm() or:
master.mav.command_long_send(master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0,
1, 0, 0, 0, 0, 0, 0)
def get_all_params(mavconn):
'''given a mavlink_connection, gets the parameters with the names specified in params'''
received_params = [] # send the "get all parameters" request
mavconn.param_fetch_all()
while mavconn.param_fetch_complete():
msg = mavconn.recv_match(type='PARAM_VALUE', blocking=True, timeout=1)
received_params.append(msg)
return received_params
# create a mavlink serial instance
master = mavutil.mavlink_connection(args.device,
baud=args.baudrate,
source_system=args.SOURCE_SYSTEM)
params = master.param_fetch_all()
wait_id(master)
# params = get_all_params(master)
# print(params)
# for p in params:# don't print None params - ones we requested that didn't exist
# if not p:
# continue
# print(str(p.param_id),'\t', p.param_value )
master.close()
# wait for the heartbeat msg to find the system ID
# wait_heartbeat(master)
## Get the autopilot version
from pymavlink import mavutil
import beepy
mavlink = mavutil.mavlink_connection('udpin:0.0.0.0:14540')
mavlink.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (mavlink.target_system, mavlink.target_component))
mavlink.mav.set_mode_send(mavlink.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 11)
print("cekam na odpoved")
while True:
# Wait for ACK command
ack_msg = mavlink.recv_match(type='COMMAND_ACK', blocking=True)
ack_msg = ack_msg.to_dict()
print("ACK", ack_msg)
# Check if command in the same in `set_mode`
if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
continue
# Print the ACK result !
print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description)
break
print("Prepnuti dokonceno")
def fly_CopterAVC(binary,
viewerip=None,
use_map=False,
valgrind=False,
gdb=False,
frame=None,
params=None,
gdbserver=False,
speedup=10):
"""Fly ArduCopter in SITL for AVC2013 mission."""
global homeloc
if frame is None:
frame = 'heli'
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt,
AVCHOME.heading)
sitl = util.start_SITL(binary,
wipe=True,
model=frame,
home=home,
speedup=speedup)
mavproxy = util.start_MAVProxy_SITL(
'ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
if params is None:
params = vinfo.options["ArduCopter"]["frames"][frame][
"default_params_filename"]
if not isinstance(params, list):
params = [params]
for x in params:
mavproxy.send("param load %s\n" % os.path.join(testdir, x))
mavproxy.expect('Loaded [0-9]+ parameters')
mavproxy.send("param set LOG_REPLAY 1\n")
mavproxy.send("param set LOG_DISARMED 1\n")
time.sleep(3)
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sitl)
sitl = util.start_SITL(binary,
model='heli',
home=home,
speedup=speedup,
valgrind=valgrind,
gdb=gdb,
gdbserver=gdbserver)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if use_map:
options += ' --map'
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)\r\n')
logfile = mavproxy.match.group(1)
progress("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
progress("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# 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, expect_callback)
expect_list_clear()
expect_list_extend([sitl, mavproxy])
if use_map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception as msg:
progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
failed_test_msg = "None"
try:
mav.wait_heartbeat()
setup_rc(mavproxy)
homeloc = mav.location()
progress("Lowering rotor speed")
mavproxy.send('rc 8 1000\n')
wait_ready_to_arm(mav)
# Arm
progress("# Arm motors")
if not arm_motors(mavproxy, mav):
failed_test_msg = "arm_motors failed"
progress(failed_test_msg)
failed = True
progress("Raising rotor speed")
mavproxy.send('rc 8 2000\n')
progress("# Fly AVC mission")
if not fly_avc_test(mavproxy, mav):
failed_test_msg = "fly_avc_test failed"
progress(failed_test_msg)
failed = True
else:
progress("Flew AVC mission OK")
progress("Lowering rotor speed")
mavproxy.send('rc 8 1000\n')
# mission includes disarm at end so should be ok to download logs now
if not log_download(mavproxy, mav,
util.reltopdir("../buildlogs/CopterAVC-log.bin")):
failed_test_msg = "log_download failed"
progress(failed_test_msg)
failed = True
except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout"
failed = True
mav.close()
util.pexpect_close(mavproxy)
util.pexpect_close(sitl)
valgrind_log = util.valgrind_log_filepath(binary=binary, model='heli')
if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0o644)
shutil.copy(valgrind_log,
util.reltopdir("../buildlogs/Helicopter-valgrind.log"))
if failed:
progress("FAILED: %s" % failed_test_msg)
return False
return True
"""
Example of how to read all the parameters from the Autopilot with pymavlink
"""
# Disable "Broad exception" warning
# pylint: disable=W0703
import time
import sys
# Import mavutil
from pymavlink import mavutil
# Create the connection
master = mavutil.mavlink_connection('tcp:127.0.0.1:5762')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# Request all parameters
master.mav.param_request_list_send(master.target_system,
master.target_component)
while True:
time.sleep(0.01)
try:
message = master.recv_match(type='PARAM_VALUE',
blocking=True).to_dict()
print('name: %s\tvalue: %d' %
(message['param_id'], message['param_value']))
except Exception as error:
print(error)
sys.exit(0)
import socket
class Drone():
def __init__(self):
self.posx = []
self.posy = []
self.posz = []
self.last_send_ts = 0
self.tracked = False
drones = [Drone() for i in range(20)]
sampling_period = 1.0 / 120.0
uwb_anchor = mavutil.mavlink_connection(device="com3",
baud=3000000,
source_system=255)
uwb_last_send_ts = 0
last_sys_time = 0
start_ts = time.time()
msgs = deque()
# This is a callback function that gets connected to the NatNet client and called once per mocap frame.
def receiveNewFrame(frameNumber, markerSetCount, unlabeledMarkersCount,
rigidBodyCount, skeletonCount, labeledMarkerCount,
timecode, timecodeSub, timestamp, isRecording,
trackedModelsChanged):
print("Received frame", frameNumber)
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)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW','GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW','GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'GPS'])
if len(types) == 1:
types.extend(['GPS','GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if m.get_type() == 'AHR2':
if opts.dualgps:
instance = 2
else:
instance = 1
if lat != 0 or lng != 0:
if getattr(mlog, 'flightmode','') in colourmap:
colour = colourmap[mlog.flightmode]
(r,g,b) = colour
(r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r,g,b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(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_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
for m in mission_obj:
map.add_object(m)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
UAV_SUBNET="192.168.90.255"
DEFAULT_UAV_PORT=14550
#TODO: need targets based on UAVID
uav_ids = [2,4]
ids_to_remove = []
uav_params = {}
target_component = 1
param_provider = "A/P" #ATMY or A/P
done_checking = False
previous_rcv_time = time.time()
mavutil.set_dialect("gtri_uav")
master = mavutil.mavlink_connection(
"udpin:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
#"udpbcast:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
out = mavutil.mavlink_connection(
"udpbcast:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
def uint_to_float(val_int):
s = struct.pack(">I", val_int)
return float(struct.unpack(">f", s)[0])
def uint_to_int(val_int):
s = struct.pack(">I", val_int)
return struct.unpack(">i", s)[0]
def to_uint(val):
if type(val) == float:
def flight_modes(logfile):
'''show flight modes for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
mode = ""
previous_mode = ""
mode_start_timestamp = -1
time_in_mode = {}
previous_percent = -1
seconds_per_percent = -1
filesize = os.path.getsize(filename)
while True:
m = mlog.recv_match(type=['SYS_STATUS', 'HEARTBEAT', 'MODE'],
condition='MAV.flightmode!="%s"' % mlog.flightmode)
if m is None:
break
print('%s MAV.flightmode=%-12s (MAV.timestamp=%u %u%%)' %
(time.asctime(time.localtime(
m._timestamp)), mlog.flightmode, m._timestamp, mlog.percent))
mode = mlog.flightmode
if (mode not in time_in_mode):
time_in_mode[mode] = 0
if (mode_start_timestamp == -1):
mode_start_timestamp = m._timestamp
elif (previous_mode != "" and previous_mode != mode):
time_in_mode[previous_mode] = time_in_mode[previous_mode] + (
m._timestamp - mode_start_timestamp)
#figure out how many seconds per percentage point so I can
#calculate how many seconds for the final mode
if (seconds_per_percent == -1 and previous_percent != -1
and previous_percent != mlog.percent):
seconds_per_percent = (m._timestamp - mode_start_timestamp) / (
mlog.percent - previous_percent)
mode_start_timestamp = m._timestamp
previous_mode = mode
previous_percent = mlog.percent
#put a whitespace line before the per-mode report
print()
print("Time per mode:")
#need to get the time in the final mode
if (seconds_per_percent != -1):
seconds_remaining = (100.0 - previous_percent) * seconds_per_percent
time_in_mode[
previous_mode] = time_in_mode[previous_mode] + seconds_remaining
total_flight_time = 0
for key, value in time_in_mode.items():
total_flight_time = total_flight_time + value
for key, value in time_in_mode.items():
print('%-12s %s %.2f%%' %
(key, str(datetime.timedelta(seconds=int(value))),
(value / total_flight_time) * 100.0))
else:
#can't print time in mode if only one mode during flight
print(previous_mode, " 100% of flight time")
# open master link
for mdev in opts.master:
if not mpstate.module('link').link_add(mdev):
sys.exit(1)
if not opts.master and len(serial_list) == 1:
print("Connecting to %s" % serial_list[0])
mpstate.module('link').link_add(serial_list[0].device)
elif not opts.master:
wifi_device = '0.0.0.0:14550'
mpstate.module('link').link_add(wifi_device)
# open any mavlink output ports
for port in opts.output:
mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(opts.baudrate), input=False))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
if opts.state_basedir is not None:
mpstate.settings.state_basedir = opts.state_basedir
msg_period = mavutil.periodic_event(1.0/15)
heartbeat_period = mavutil.periodic_event(1)
heartbeat_check_period = mavutil.periodic_event(0.33)
mpstate.input_queue = Queue.Queue()
def drive_APMrover2(viewerip=None, map=False):
'''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
if map:
options += ' --map'
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py'
) + ' --rate=200 --nowait --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2',
options=options,
synthetic_clock=True)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
runsim.expect('Starting at lat')
print("WAITING FOR PARAMETERS")
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
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)
util.pexpect_close(runsim)
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2',
options=options,
synthetic_clock=True)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py'
) + ' --rate=200 --nowait --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')
buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
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
import numpy as np
import serial
from datetime import datetime
SIMULATOR_UDP = 'udp:127.0.0.1:14550'
UDP_IP_LOCAL = "127.0.0.1"
UDP_PORT = 2992
UDP_IP_650_TCP = 'tcp:192.168.0.210:20002'
RPI_SERIAL = '/dev/ttyS0'
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP_LOCAL, UDP_PORT))
master = mavutil.mavlink_connection(UDP_IP_650_TCP, dialect="ardupilotmega")
#master = mavutil.mavlink_connection(RPI_SERIAL, baud = 115200)
#mavutil.set_dialect("ardupilotmega")
ser = serial.Serial(port='/dev/ttyS0',
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
timeout=0)
def timeFrom00(timeInInt):
miSec = timeInInt % 1000000
timeInInt = timeInInt / 1000000
sec = timeInInt % 100
timeInInt = timeInInt / 100
print("Connecting to %s" % serial_list[0])
mpstate.module('link').link_add(serial_list[0].device)
elif not opts.master and len(serial_list) > 1:
print(
"Error: multiple possible serial ports; use --master to select a single port"
)
sys.exit(1)
elif not opts.master:
wifi_device = '0.0.0.0:14550'
mpstate.module('link').link_add(wifi_device)
# open any mavlink output ports
for port in opts.output:
mpstate.mav_outputs.append(
mavutil.mavlink_connection(port,
baud=int(opts.baudrate),
input=False))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
if opts.state_basedir is not None:
mpstate.settings.state_basedir = opts.state_basedir
msg_period = mavutil.periodic_event(1.0 / 15)
heartbeat_period = mavutil.periodic_event(1)
heartbeat_check_period = mavutil.periodic_event(0.33)
def magfit(logfile):
'''find best magnetometer rotation fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
last_mag = None
last_usec = 0
count = 0
total_error = [0]*len(rotations)
AHRS_ORIENTATION = 0
COMPASS_ORIENT = 0
COMPASS_EXTERNAL = 0
last_gyr = None
# now gather all the data
while True:
m = mlog.recv_match()
if m is None:
break
if m.get_type() in ["PARAM_VALUE", "PARM"]:
if m.get_type() == "PARM":
name = str(m.Name)
value = int(m.Value)
else:
name = str(m.param_id)
value = int(m.param_value)
if name == "AHRS_ORIENTATION":
AHRS_ORIENTATION = value
if name == 'COMPASS_ORIENT':
COMPASS_ORIENT = value
if name == 'COMPASS_EXTERNAL':
COMPASS_EXTERNAL = value
if m.get_type() in ["RAW_IMU", "MAG","IMU"]:
if m.get_type() == "RAW_IMU":
mag = Vector3(m.xmag, m.ymag, m.zmag)
gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001
usec = m.time_usec
elif m.get_type() == "IMU":
last_gyr = Vector3(m.GyrX,m.GyrY,m.GyrZ)
continue
elif last_gyr is not None:
mag = Vector3(m.MagX,m.MagY,m.MagZ)
gyr = last_gyr
usec = m.TimeMS * 1000
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL)
if last_mag is not None and gyr.length() > radians(opts.min_rotation):
add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations)
count += 1
last_mag = mag
last_usec = usec
best_i = 0
best_err = total_error[0]
for i in range(len(rotations)):
r = rotations[i]
if not r.is_90_degrees():
continue
if opts.verbose:
print("%s err=%.2f" % (r, total_error[i]/count))
if total_error[i] < best_err:
best_i = i
best_err = total_error[i]
r = rotations[best_i]
print("Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u" % (
rotations[AHRS_ORIENTATION],
rotations[COMPASS_ORIENT],
COMPASS_EXTERNAL))
print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count))
print("Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" % (
rotations[AHRS_ORIENTATION],
r))
def playback(mavcon, images, targets, target_lat, target_lon, C_params):
'''create synthetic images matching target position'''
print("Connecting to %s" % mavcon)
mlog = mavutil.mavlink_connection(mavcon)
# get first message
msg = mlog.recv_match(type='ATTITUDE', blocking=True)
mavtime = msg.time_boot_ms * 0.001
last_print = time.time()
last_image = mavtime
mpos = mav_position.MavInterpolator()
Xres = 1640
Yres = 1232
tmpimages = "tmpimages"
print("Using %s directory" % tmpimages)
try:
os.mkdir(tmpimages)
except Exception:
pass
while True:
msg = mlog.recv_match(blocking=False)
if not msg:
time.sleep(0.1)
continue
mpos.add_msg(msg)
mtype = msg.get_type()
if mtype == "ATTITUDE":
mavtime = msg.time_boot_ms * 0.001
if mavtime - last_image > 0.8:
last_image = mavtime
if not 'GLOBAL_POSITION_INT' in mlog.messages:
continue
frame_time = time.time() - 2.0
try:
pos = mpos.position(frame_time)
except Exception:
continue
imgfile = random.choice(images)
img = cv2.imread(imgfile)
filename = os.path.join(
tmpimages,
cuav_util.frame_time(frame_time) + ".jpg")
# see if the target should be in the image
xy = find_xy_in_image(target_lat, target_lon, pos, Xres, Yres,
C_params)
if xy is None:
# when we are not modifying the image use a hardlink to save space
os.link(imgfile, filename)
else:
# overlay a random target on the image
(x, y) = xy
tgtfile = random.choice(targets)
# targets come from an 80m reference height
tgt_scale = 80.0 / max(pos.altitude, 1)
tgt = cv2.imread(tgtfile)
tgt = cv2.resize(tgt, (0, 0), fx=tgt_scale, fy=tgt_scale)
try:
img[y:y + tgt.shape[0], x:x + tgt.shape[1]] = tgt
cv2.imwrite(filename, img)
except Exception:
os.link(imgfile, filename)
# create symlink
try:
os.unlink("capture.jpg")
except Exception:
pass
os.symlink(filename, "capture.jpg")
print(filename, xy)
"""
msg = None
while not msg:
master.mav.ping_send(
int(time.time() * 1e6), # Unix time in microseconds
0, # Ping number
0, # Request ping of all systems
0 # Request ping of all components
)
msg = master.recv_match()
time.sleep(0.5)
# Connect to the default listening port for
# mavproxy on Blue Robotics companion computer
master = mavutil.mavlink_connection('udpout:localhost:9000')
# Send a ping to start connection and wait for any reply.
wait_conn()
# Configure the autopilot to use mavlink rangefinder, the autopilot
# will need to be rebooted after this to use the updated setting
master.mav.param_set_send(
1,
1,
b"RNGFND_TYPE",
10, # "MAVLink"
mavutil.mavlink.MAV_PARAM_TYPE_INT8)
min_measurement = 10 # minimum valid measurement that the autopilot should use
max_measurement = 40 # maximum valid measurement that the autopilot should use
from pymavlink import mavutil, mavwp
master = mavutil.mavlink_connection('udp:localhost:14551')
master.wait_heartbeat(blocking=True)
wp = mavwp.MAVWPLoader()
seq = 1
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
radius = 10
for i in range(N):
wp.add(
mavutil.mavlink.MAVLink_mission_item_message(
master.target_system, master.target_component, seq, frame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, radius, 0, 0,
lat[i], lon[i], alt[i]))
seq += 1
master.waypoint_clear_all_send()
master.waypoint_count_send(wp.count())
for i in range(wp.count()):
msg = master.recv_match(type=['MISSION_REQUEST'], blocking=True)
master.mav.send(wp.wp(msg.seq))
print 'Sending waypoint {0}'.format(msg.seq)
parser.add_argument("--zero-time-base", action='store_true', help="use Z time base for DF logs")
parser.add_argument("--show-flightmode", default=True,
help="Add background colour to plot corresponding to current flight mode. Cannot be specified with --xaxis.")
parser.add_argument("--dialect", default="ardupilotmega", help="MAVLink dialect")
parser.add_argument("--output", default=None, help="provide an output format")
parser.add_argument("--timeshift", type=float, default=0, help="shift time on first graph in seconds")
parser.add_argument("logs_fields", metavar="<LOG or FIELD>", nargs="+")
args = parser.parse_args()
mg = MavGraph()
filenames = []
for f in args.logs_fields:
if os.path.exists(f):
mlog = mavutil.mavlink_connection(f, notimestamps=args.notimestamps,
zero_time_base=args.zero_time_base,
dialect=args.dialect)
mg.add_mav(mlog)
else:
mg.add_field(f)
mg.set_condition(args.condition)
mg.set_xaxis(args.xaxis)
mg.set_marker(args.marker)
mg.set_legend(args.legend)
mg.set_legend2(args.legend2)
mg.set_multi(args.multi)
mg.set_title(args.title)
mg.set_show_flightmode(args.show_flightmode)
mg.process([],[],0)
mg.show(len(mg.mav_list), output=args.output)