mavutil.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 '''
3 mavlink python utility functions
4 
5 Copyright Andrew Tridgell 2011
6 Released under GNU GPL version 3 or later
7 '''
8 from __future__ import print_function
9 from builtins import object
10 
11 import socket, math, struct, time, os, fnmatch, array, sys, errno
12 import select
13 from pymavlink import mavexpression
14 
15 # adding these extra imports allows pymavlink to be used directly with pyinstaller
16 # without having complex spec files. To allow for installs that don't have ardupilotmega
17 # at all we avoid throwing an exception if it isn't installed
18 try:
19  import json
20  from pymavlink.dialects.v10 import ardupilotmega
21 except Exception:
22  pass
23 
24 # maximum packet length for a single receive call - use the UDP limit
25 UDP_MAX_PACKET_LEN = 65535
26 
27 # Store the MAVLink library for the currently-selected dialect
28 # (set by set_dialect())
29 mavlink = None
30 
31 # Store the mavlink file currently being operated on
32 # (set by mavlink_connection())
33 mavfile_global = None
34 
35 # If the caller hasn't specified a particular native/legacy version, use this
36 default_native = False
37 
38 # link_id used for signing
39 global_link_id = 0
40 
41 # Use a globally-set MAVLink dialect if one has been specified as an environment variable.
42 if not 'MAVLINK_DIALECT' in os.environ:
43  os.environ['MAVLINK_DIALECT'] = 'ardupilotmega'
44 
45 def mavlink10():
46  '''return True if using MAVLink 1.0 or later'''
47  return not 'MAVLINK09' in os.environ
48 
49 def mavlink20():
50  '''return True if using MAVLink 2.0'''
51  return 'MAVLINK20' in os.environ
52 
53 def evaluate_expression(expression, vars):
54  '''evaluation an expression'''
55  return mavexpression.evaluate_expression(expression, vars)
56 
57 def evaluate_condition(condition, vars):
58  '''evaluation a conditional (boolean) statement'''
59  if condition is None:
60  return True
61  v = evaluate_expression(condition, vars)
62  if v is None:
63  return False
64  return v
65 
66 def u_ord(c):
67  return ord(c) if sys.version_info.major < 3 else c
68 
69 class location(object):
70  '''represent a GPS coordinate'''
71  def __init__(self, lat, lng, alt=0, heading=0):
72  self.lat = lat
73  self.lng = lng
74  self.alt = alt
75  self.heading = heading
76 
77  def __str__(self):
78  return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
79 
80 def set_dialect(dialect):
81  '''set the MAVLink dialect to work with.
82  For example, set_dialect("ardupilotmega")
83  '''
84  global mavlink, current_dialect
85  from .generator import mavparse
86  if 'MAVLINK20' in os.environ:
87  wire_protocol = mavparse.PROTOCOL_2_0
88  modname = "pymavlink.dialects.v20." + dialect
89  elif mavlink is None or mavlink.WIRE_PROTOCOL_VERSION == "1.0" or not 'MAVLINK09' in os.environ:
90  wire_protocol = mavparse.PROTOCOL_1_0
91  modname = "pymavlink.dialects.v10." + dialect
92  else:
93  wire_protocol = mavparse.PROTOCOL_0_9
94  modname = "pymavlink.dialects.v09." + dialect
95 
96  try:
97  mod = __import__(modname)
98  except Exception:
99  # auto-generate the dialect module
100  from .generator.mavgen import mavgen_python_dialect
101  mavgen_python_dialect(dialect, wire_protocol)
102  mod = __import__(modname)
103  components = modname.split('.')
104  for comp in components[1:]:
105  mod = getattr(mod, comp)
106  current_dialect = dialect
107  mavlink = mod
108 
109 # Set the default dialect. This is done here as it needs to be after the function declaration
110 set_dialect(os.environ['MAVLINK_DIALECT'])
111 
112 class mavfile_state(object):
113  '''state for a particular system id'''
114  def __init__(self):
115  self.messages = { 'MAV' : self }
116  self.flightmode = "UNKNOWN"
117  self.vehicle_type = "UNKNOWN"
118  self.mav_type = mavlink.MAV_TYPE_FIXED_WING
119  self.base_mode = 0
120  self.armed = False # canonical arm state for the vehicle as a whole
121 
122  if float(mavlink.WIRE_PROTOCOL_VERSION) >= 1:
123  self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0)
124  mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message
125  else:
126  self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
127 
128 class param_state(object):
129  '''state for a particular system id/component id pair'''
130  def __init__(self):
131  self.params = {}
132 
133 class mavfile(object):
134  '''a generic mavlink port'''
135  def __init__(self, fd, address, source_system=255, source_component=0, notimestamps=False, input=True, use_native=default_native):
136  global mavfile_global
137  if input:
138  mavfile_global = self
139  self.fd = fd
140  self.sysid = 0
141  self.param_sysid = (0,0)
142  self.address = address
143  self.timestamp = 0
144  self.last_seq = {}
145  self.mav_loss = 0
146  self.mav_count = 0
147  self.param_fetch_start = 0
148 
149  # state for each sysid
150  self.sysid_state = {}
151  self.sysid_state[self.sysid] = mavfile_state()
152 
153  # param state for each sysid/compid tuple
154  self.param_state = {}
155  self.param_state[self.param_sysid] = param_state()
156 
157  # status of param fetch, indexed by sysid,compid tuple
158  self.source_system = source_system
159  self.source_component = source_component
160  self.first_byte = True
161  self.robust_parsing = True
162  self.mav = mavlink.MAVLink(self, srcSystem=self.source_system, srcComponent=self.source_component, use_native=use_native)
163  self.mav.robust_parsing = self.robust_parsing
164  self.logfile = None
165  self.logfile_raw = None
166  self.start_time = time.time()
167  self.message_hooks = []
168  self.idle_hooks = []
169  self.uptime = 0.0
170  self.notimestamps = notimestamps
171  self._timestamp = None
172  self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
173  self.stop_on_EOF = False
174  self.portdead = False
175 
176  @property
177  def target_system(self):
178  return self.sysid
179 
180  @property
181  def target_component(self):
182  return self.param_sysid[1]
183 
184  @target_system.setter
185  def target_system(self, value):
186  self.sysid = value
187  if not self.sysid in self.sysid_state:
188  self.sysid_state[self.sysid] = mavfile_state()
189  if self.sysid != self.param_sysid[0]:
190  self.param_sysid = (self.sysid, self.param_sysid[1])
191  if not self.param_sysid in self.param_state:
192  self.param_state[self.param_sysid] = param_state()
193 
194  @target_component.setter
195  def target_component(self, value):
196  if value != self.param_sysid[1]:
197  self.param_sysid = (self.param_sysid[0], value)
198  if not self.param_sysid in self.param_state:
199  self.param_state[self.param_sysid] = param_state()
200 
201  @property
202  def params(self):
203  if self.param_sysid[1] == 0:
204  eff_tuple = (self.sysid, 1)
205  if eff_tuple in self.param_state:
206  return getattr(self.param_state[eff_tuple],'params')
207  return getattr(self.param_state[self.param_sysid],'params')
208 
209  @property
210  def messages(self):
211  return getattr(self.sysid_state[self.sysid],'messages')
212 
213  @property
214  def flightmode(self):
215  return getattr(self.sysid_state[self.sysid],'flightmode')
216 
217  @flightmode.setter
218  def flightmode(self, value):
219  setattr(self.sysid_state[self.sysid],'flightmode',value)
220 
221  @property
222  def vehicle_type(self):
223  return getattr(self.sysid_state[self.sysid],'vehicle_type')
224 
225  @vehicle_type.setter
226  def vehicle_type(self, value):
227  setattr(self.sysid_state[self.sysid],'vehicle_type',value)
228 
229  @property
230  def mav_type(self):
231  return getattr(self.sysid_state[self.sysid],'mav_type')
232 
233  @mav_type.setter
234  def mav_type(self, value):
235  setattr(self.sysid_state[self.sysid],'mav_type',value)
236 
237  @property
238  def base_mode(self):
239  return getattr(self.sysid_state[self.sysid],'base_mode')
240 
241  @base_mode.setter
242  def base_mode(self, value):
243  setattr(self.sysid_state[self.sysid],'base_mode',value)
244 
245  def auto_mavlink_version(self, buf):
246  '''auto-switch mavlink protocol version'''
247  global mavlink
248  if len(buf) == 0:
249  return
250  try:
251  magic = ord(buf[0])
252  except:
253  magic = buf[0]
254  if not magic in [ 85, 254, 253 ]:
255  return
256  self.first_byte = False
257  if self.WIRE_PROTOCOL_VERSION == "0.9" and magic == 254:
258  self.WIRE_PROTOCOL_VERSION = "1.0"
259  set_dialect(current_dialect)
260  elif self.WIRE_PROTOCOL_VERSION == "1.0" and magic == 85:
261  self.WIRE_PROTOCOL_VERSION = "0.9"
262  os.environ['MAVLINK09'] = '1'
263  set_dialect(current_dialect)
264  elif self.WIRE_PROTOCOL_VERSION != "2.0" and magic == 253:
265  self.WIRE_PROTOCOL_VERSION = "2.0"
266  os.environ['MAVLINK20'] = '1'
267  set_dialect(current_dialect)
268  else:
269  return
270  # switch protocol
271  (callback, callback_args, callback_kwargs) = (self.mav.callback,
272  self.mav.callback_args,
273  self.mav.callback_kwargs)
274  self.mav = mavlink.MAVLink(self, srcSystem=self.source_system, srcComponent=self.source_component)
275  self.mav.robust_parsing = self.robust_parsing
276  self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
277  (self.mav.callback, self.mav.callback_args, self.mav.callback_kwargs) = (callback,
278  callback_args,
279  callback_kwargs)
280 
281  def recv(self, n=None):
282  '''default recv method'''
283  raise RuntimeError('no recv() method supplied')
284 
285  def close(self, n=None):
286  '''default close method'''
287  raise RuntimeError('no close() method supplied')
288 
289  def write(self, buf):
290  '''default write method'''
291  raise RuntimeError('no write() method supplied')
292 
293 
294  def select(self, timeout):
295  '''wait for up to timeout seconds for more data'''
296  if self.fd is None:
297  time.sleep(min(timeout,0.5))
298  return True
299  try:
300  (rin, win, xin) = select.select([self.fd], [], [], timeout)
301  except select.error:
302  return False
303  return len(rin) == 1
304 
305  def pre_message(self):
306  '''default pre message call'''
307  return
308 
309  def set_rtscts(self, enable):
310  '''enable/disable RTS/CTS if applicable'''
311  return
312 
313  def probably_vehicle_heartbeat(self, msg):
314  if msg.get_srcComponent() == mavlink.MAV_COMP_ID_GIMBAL:
315  return False
316  if msg.type in (mavlink.MAV_TYPE_GCS,
317  mavlink.MAV_TYPE_GIMBAL,
318  mavlink.MAV_TYPE_ADSB,
319  mavlink.MAV_TYPE_ONBOARD_CONTROLLER):
320  return False
321  return True
322 
323  def post_message(self, msg):
324  '''default post message call'''
325  if '_posted' in msg.__dict__:
326  return
327  msg._posted = True
328  msg._timestamp = time.time()
329  type = msg.get_type()
330 
331  if 'usec' in msg.__dict__:
332  self.uptime = msg.usec * 1.0e-6
333  if 'time_boot_ms' in msg.__dict__:
334  self.uptime = msg.time_boot_ms * 1.0e-3
335 
336  if self._timestamp is not None:
337  if self.notimestamps:
338  msg._timestamp = self.uptime
339  else:
340  msg._timestamp = self._timestamp
341 
342  src_system = msg.get_srcSystem()
343  src_component = msg.get_srcComponent()
344  src_tuple = (src_system, src_component)
345 
346  radio_tuple = (ord('3'), ord('D'))
347 
348  if not src_system in self.sysid_state:
349  # we've seen a new system
350  self.sysid_state[src_system] = mavfile_state()
351 
352  self.sysid_state[src_system].messages[type] = msg
353 
354  if src_tuple == radio_tuple:
355  # as a special case radio msgs are added for all sysids
356  for s in self.sysid_state.keys():
357  self.sysid_state[s].messages[type] = msg
358 
359  if not (src_tuple == radio_tuple or msg.get_type() == 'BAD_DATA'):
360  if not src_tuple in self.last_seq:
361  last_seq = -1
362  else:
363  last_seq = self.last_seq[src_tuple]
364  seq = (last_seq+1) % 256
365  seq2 = msg.get_seq()
366  if seq != seq2 and last_seq != -1:
367  diff = (seq2 - seq) % 256
368  self.mav_loss += diff
369  #print("lost %u seq=%u seq2=%u last_seq=%u src_tupe=%s %s" % (diff, seq, seq2, last_seq, str(src_tuple), msg.get_type()))
370  self.last_seq[src_tuple] = seq2
371  self.mav_count += 1
372 
373  self.timestamp = msg._timestamp
374  if type == 'HEARTBEAT' and self.probably_vehicle_heartbeat(msg):
375  if self.sysid == 0:
376  # lock onto id tuple of first vehicle heartbeat
377  self.sysid = src_system
378  if float(mavlink.WIRE_PROTOCOL_VERSION) >= 1:
379  self.flightmode = mode_string_v10(msg)
380  self.mav_type = msg.type
381  self.base_mode = msg.base_mode
382  self.sysid_state[self.sysid].armed = (msg.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
383 
384  elif type == 'PARAM_VALUE':
385  if not src_tuple in self.param_state:
386  self.param_state[src_tuple] = param_state()
387  self.param_state[src_tuple].params[msg.param_id] = msg.param_value
388  elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
389  self.flightmode = mode_string_v09(msg)
390  elif type == 'GPS_RAW':
391  if self.sysid_state[src_system].messages['HOME'].fix_type < 2:
392  self.sysid_state[src_system].messages['HOME'] = msg
393  elif type == 'GPS_RAW_INT':
394  if self.sysid_state[src_system].messages['HOME'].fix_type < 3:
395  self.sysid_state[src_system].messages['HOME'] = msg
396  for hook in self.message_hooks:
397  hook(self, msg)
398 
399  if (msg.get_signed() and
400  self.mav.signing.link_id == 0 and
401  msg.get_link_id() != 0 and
402  self.target_system == msg.get_srcSystem() and
403  self.target_component == msg.get_srcComponent()):
404  # change to link_id from incoming packet
405  self.mav.signing.link_id = msg.get_link_id()
406 
407 
408  def packet_loss(self):
409  '''packet loss as a percentage'''
410  if self.mav_count == 0:
411  return 0
412  return (100.0*self.mav_loss)/(self.mav_count+self.mav_loss)
413 
414 
415  def recv_msg(self):
416  '''message receive routine'''
417  self.pre_message()
418  while True:
419  n = self.mav.bytes_needed()
420  s = self.recv(n)
421  numnew = len(s)
422 
423  if numnew != 0:
424  if self.logfile_raw:
425  self.logfile_raw.write(str(s))
426  if self.first_byte:
427  self.auto_mavlink_version(s)
428 
429  # We always call parse_char even if the new string is empty, because the existing message buf might already have some valid packet
430  # we can extract
431  msg = self.mav.parse_char(s)
432  if msg:
433  if self.logfile and msg.get_type() != 'BAD_DATA' :
434  usec = int(time.time() * 1.0e6) & ~3
435  self.logfile.write(str(struct.pack('>Q', usec) + msg.get_msgbuf()))
436  self.post_message(msg)
437  return msg
438  else:
439  # if we failed to parse any messages _and_ no new bytes arrived, return immediately so the client has the option to
440  # timeout
441  if numnew == 0:
442  return None
443 
444  def recv_match(self, condition=None, type=None, blocking=False, timeout=None):
445  '''recv the next MAVLink message that matches the given condition
446  type can be a string or a list of strings'''
447  if type is not None and not isinstance(type, list) and not isinstance(type, set):
448  type = [type]
449  start_time = time.time()
450  while True:
451  if timeout is not None:
452  now = time.time()
453  if now < start_time:
454  start_time = now # If an external process rolls back system time, we should not spin forever.
455  if start_time + timeout < time.time():
456  return None
457  m = self.recv_msg()
458  if m is None:
459  if blocking:
460  for hook in self.idle_hooks:
461  hook(self)
462  if timeout is None:
463  self.select(0.05)
464  else:
465  self.select(timeout/2)
466  continue
467  return None
468  if type is not None and not m.get_type() in type:
469  continue
470  if not evaluate_condition(condition, self.messages):
471  continue
472  return m
473 
474  def check_condition(self, condition):
475  '''check if a condition is true'''
476  return evaluate_condition(condition, self.messages)
477 
478  def mavlink10(self):
479  '''return True if using MAVLink 1.0 or later'''
480  return float(self.WIRE_PROTOCOL_VERSION) >= 1
481 
482  def mavlink20(self):
483  '''return True if using MAVLink 2.0 or later'''
484  return float(self.WIRE_PROTOCOL_VERSION) >= 2
485 
486  def setup_logfile(self, logfile, mode='w'):
487  '''start logging to the given logfile, with timestamps'''
488  self.logfile = open(logfile, mode=mode)
489 
490  def setup_logfile_raw(self, logfile, mode='w'):
491  '''start logging raw bytes to the given logfile, without timestamps'''
492  self.logfile_raw = open(logfile, mode=mode)
493 
494  def wait_heartbeat(self, blocking=True, timeout=None):
495  '''wait for a heartbeat so we know the target system IDs'''
496  return self.recv_match(type='HEARTBEAT', blocking=blocking, timeout=timeout)
497 
498  def param_fetch_all(self):
499  '''initiate fetch of all parameters'''
500  if time.time() - self.param_fetch_start < 2.0:
501  # don't fetch too often
502  return
503  self.param_fetch_start = time.time()
504  self.mav.param_request_list_send(self.target_system, self.target_component)
505 
506  def param_fetch_one(self, name):
507  '''initiate fetch of one parameter'''
508  try:
509  idx = int(name)
510  self.mav.param_request_read_send(self.target_system, self.target_component, b"", idx)
511  except Exception:
512  if sys.version_info.major >= 3 and not isinstance(name, bytes):
513  name = bytes(name,'ascii')
514  self.mav.param_request_read_send(self.target_system, self.target_component, name, -1)
515 
516  def time_since(self, mtype):
517  '''return the time since the last message of type mtype was received'''
518  if not mtype in self.messages:
519  return time.time() - self.start_time
520  return time.time() - self.messages[mtype]._timestamp
521 
522  def param_set_send(self, parm_name, parm_value, parm_type=None):
523  '''wrapper for parameter set'''
524  if self.mavlink10():
525  if parm_type is None:
526  parm_type = mavlink.MAVLINK_TYPE_FLOAT
527  self.mav.param_set_send(self.target_system, self.target_component,
528  parm_name.encode('utf8'), parm_value, parm_type)
529  else:
530  self.mav.param_set_send(self.target_system, self.target_component,
531  parm_name.encode('utf8'), parm_value)
532 
533  def waypoint_request_list_send(self):
534  '''wrapper for waypoint_request_list_send'''
535  if self.mavlink10():
536  self.mav.mission_request_list_send(self.target_system, self.target_component)
537  else:
538  self.mav.waypoint_request_list_send(self.target_system, self.target_component)
539 
540  def waypoint_clear_all_send(self):
541  '''wrapper for waypoint_clear_all_send'''
542  if self.mavlink10():
543  self.mav.mission_clear_all_send(self.target_system, self.target_component)
544  else:
545  self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
546 
547  def waypoint_request_send(self, seq):
548  '''wrapper for waypoint_request_send'''
549  if self.mavlink10():
550  self.mav.mission_request_send(self.target_system, self.target_component, seq)
551  else:
552  self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
553 
554  def waypoint_set_current_send(self, seq):
555  '''wrapper for waypoint_set_current_send'''
556  if self.mavlink10():
557  self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
558  else:
559  self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
560 
561  def waypoint_current(self):
562  '''return current waypoint'''
563  if self.mavlink10():
564  m = self.recv_match(type='MISSION_CURRENT', blocking=True)
565  else:
566  m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
567  return m.seq
568 
569  def waypoint_count_send(self, seq):
570  '''wrapper for waypoint_count_send'''
571  if self.mavlink10():
572  self.mav.mission_count_send(self.target_system, self.target_component, seq)
573  else:
574  self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
575 
576  def set_mode_flag(self, flag, enable):
577  '''
578  Enables/ disables MAV_MODE_FLAG
579  @param flag The mode flag,
580  see MAV_MODE_FLAG enum
581  @param enable Enable the flag, (True/False)
582  '''
583  if self.mavlink10():
584  mode = self.base_mode
585  if enable:
586  mode = mode | flag
587  elif not enable:
588  mode = mode & ~flag
589  self.mav.command_long_send(self.target_system, self.target_component,
590  mavlink.MAV_CMD_DO_SET_MODE, 0,
591  mode,
592  0, 0, 0, 0, 0, 0)
593  else:
594  print("Set mode flag not supported")
595 
596  def set_mode_auto(self):
597  '''enter auto mode'''
598  if self.mavlink10():
599  self.mav.command_long_send(self.target_system, self.target_component,
600  mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0)
601  else:
602  MAV_ACTION_SET_AUTO = 13
603  self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_SET_AUTO)
604 
605  def mode_mapping(self):
606  '''return dictionary mapping mode names to numbers, or None if unknown'''
607  mav_type = self.field('HEARTBEAT', 'type', self.mav_type)
608  mav_autopilot = self.field('HEARTBEAT', 'autopilot', None)
609  if mav_autopilot == mavlink.MAV_AUTOPILOT_PX4:
610  return px4_map
611  if mav_type is None:
612  return None
613  map = None
614  if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
615  mavlink.MAV_TYPE_HELICOPTER,
616  mavlink.MAV_TYPE_HEXAROTOR,
617  mavlink.MAV_TYPE_OCTOROTOR,
618  mavlink.MAV_TYPE_DODECAROTOR,
619  mavlink.MAV_TYPE_COAXIAL,
620  mavlink.MAV_TYPE_TRICOPTER]:
621  map = mode_mapping_acm
622  if mav_type == mavlink.MAV_TYPE_FIXED_WING:
623  map = mode_mapping_apm
624  if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
625  map = mode_mapping_rover
626  if mav_type == mavlink.MAV_TYPE_SURFACE_BOAT:
627  map = mode_mapping_rover # for the time being
628  if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
629  map = mode_mapping_tracker
630  if mav_type == mavlink.MAV_TYPE_SUBMARINE:
631  map = mode_mapping_sub
632  if map is None:
633  return None
634  inv_map = dict((a, b) for (b, a) in map.items())
635  return inv_map
636 
637  def set_mode_apm(self, mode, custom_mode = 0, custom_sub_mode = 0):
638  '''enter arbitrary mode'''
639  if isinstance(mode, str):
640  mode_map = self.mode_mapping()
641  if mode_map is None or mode not in mode_map:
642  print("Unknown mode '%s'" % mode)
643  return
644  mode = mode_map[mode]
645  # set mode by integer mode number for ArduPilot
646  self.mav.set_mode_send(self.target_system,
647  mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
648  mode)
649 
650  def set_mode_px4(self, mode, custom_mode, custom_sub_mode):
651  '''enter arbitrary mode'''
652  if isinstance(mode, str):
653  mode_map = self.mode_mapping()
654  if mode_map is None or mode not in mode_map:
655  print("Unknown mode '%s'" % mode)
656  return
657  # PX4 uses two fields to define modes
658  mode, custom_mode, custom_sub_mode = px4_map[mode]
659  self.mav.command_long_send(self.target_system, self.target_component,
660  mavlink.MAV_CMD_DO_SET_MODE, 0, mode, custom_mode, custom_sub_mode, 0, 0, 0, 0)
661 
662  def set_mode(self, mode, custom_mode = 0, custom_sub_mode = 0):
663  '''set arbitrary flight mode'''
664  mav_autopilot = self.field('HEARTBEAT', 'autopilot', None)
665  if mav_autopilot == mavlink.MAV_AUTOPILOT_PX4:
666  self.set_mode_px4(mode, custom_mode, custom_sub_mode)
667  else:
668  self.set_mode_apm(mode)
669 
670  def set_mode_rtl(self):
671  '''enter RTL mode'''
672  if self.mavlink10():
673  self.mav.command_long_send(self.target_system, self.target_component,
674  mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
675  else:
676  MAV_ACTION_RETURN = 3
677  self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_RETURN)
678 
679  def set_mode_manual(self):
680  '''enter MANUAL mode'''
681  if self.mavlink10():
682  self.mav.command_long_send(self.target_system, self.target_component,
683  mavlink.MAV_CMD_DO_SET_MODE, 0,
684  mavlink.MAV_MODE_MANUAL_ARMED,
685  0, 0, 0, 0, 0, 0)
686  else:
687  MAV_ACTION_SET_MANUAL = 12
688  self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_SET_MANUAL)
689 
690  def set_mode_fbwa(self):
691  '''enter FBWA mode'''
692  if self.mavlink10():
693  self.mav.command_long_send(self.target_system, self.target_component,
694  mavlink.MAV_CMD_DO_SET_MODE, 0,
695  mavlink.MAV_MODE_STABILIZE_ARMED,
696  0, 0, 0, 0, 0, 0)
697  else:
698  print("Forcing FBWA not supported")
699 
700  def set_mode_loiter(self):
701  '''enter LOITER mode'''
702  if self.mavlink10():
703  self.mav.command_long_send(self.target_system, self.target_component,
704  mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0)
705  else:
706  MAV_ACTION_LOITER = 27
707  self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_LOITER)
708 
709  def set_servo(self, channel, pwm):
710  '''set a servo value'''
711  self.mav.command_long_send(self.target_system, self.target_component,
712  mavlink.MAV_CMD_DO_SET_SERVO, 0,
713  channel, pwm,
714  0, 0, 0, 0, 0)
715 
716 
717  def set_relay(self, relay_pin=0, state=True):
718  '''Set relay_pin to value of state'''
719  if self.mavlink10():
720  self.mav.command_long_send(
721  self.target_system, # target_system
722  self.target_component, # target_component
723  mavlink.MAV_CMD_DO_SET_RELAY, # command
724  0, # Confirmation
725  relay_pin, # Relay Number
726  int(state), # state (1 to indicate arm)
727  0, # param3 (all other params meaningless)
728  0, # param4
729  0, # param5
730  0, # param6
731  0) # param7
732  else:
733  print("Setting relays not supported.")
734 
735  def calibrate_level(self):
736  '''calibrate accels (1D version)'''
737  self.mav.command_long_send(self.target_system, self.target_component,
738  mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
739  1, 1, 0, 0, 0, 0, 0)
740 
741  def calibrate_pressure(self):
742  '''calibrate pressure'''
743  if self.mavlink10():
744  self.mav.command_long_send(self.target_system, self.target_component,
745  mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
746  0, 0, 1, 0, 0, 0, 0)
747  else:
748  MAV_ACTION_CALIBRATE_PRESSURE = 20
749  self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_CALIBRATE_PRESSURE)
750 
751  def reboot_autopilot(self, hold_in_bootloader=False):
752  '''reboot the autopilot'''
753  if self.mavlink10():
754  if hold_in_bootloader:
755  param1 = 3
756  else:
757  param1 = 1
758  self.mav.command_long_send(self.target_system, self.target_component,
759  mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0,
760  param1, 0, 0, 0, 0, 0, 0)
761  # send an old style reboot immediately afterwards in case it is an older firmware
762  # that doesn't understand the new convention
763  self.mav.command_long_send(self.target_system, self.target_component,
764  mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0,
765  1, 0, 0, 0, 0, 0, 0)
766 
767  def wait_gps_fix(self):
768  self.recv_match(type='VFR_HUD', blocking=True)
769  if self.mavlink10():
770  self.recv_match(type='GPS_RAW_INT', blocking=True,
771  condition='GPS_RAW_INT.fix_type>=3 and GPS_RAW_INT.lat != 0')
772  else:
773  self.recv_match(type='GPS_RAW', blocking=True,
774  condition='GPS_RAW.fix_type>=2 and GPS_RAW.lat != 0')
775 
776  def location(self, relative_alt=False):
777  '''return current location'''
778  self.wait_gps_fix()
779  # wait for another VFR_HUD, to ensure we have correct altitude
780  self.recv_match(type='VFR_HUD', blocking=True)
781  self.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
782  if relative_alt:
783  alt = self.messages['GLOBAL_POSITION_INT'].relative_alt*0.001
784  else:
785  alt = self.messages['VFR_HUD'].alt
786  return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
787  self.messages['GPS_RAW_INT'].lon*1.0e-7,
788  alt,
789  self.messages['VFR_HUD'].heading)
790 
791  def arducopter_arm(self):
792  '''arm motors (arducopter only)'''
793  if self.mavlink10():
794  self.mav.command_long_send(
795  self.target_system, # target_system
796  self.target_component,
797  mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
798  0, # confirmation
799  1, # param1 (1 to indicate arm)
800  0, # param2 (all other params meaningless)
801  0, # param3
802  0, # param4
803  0, # param5
804  0, # param6
805  0) # param7
806 
807  def arducopter_disarm(self):
808  '''calibrate pressure'''
809  if self.mavlink10():
810  self.mav.command_long_send(
811  self.target_system, # target_system
812  self.target_component,
813  mavlink.MAV_CMD_COMPONENT_ARM_DISARM, # command
814  0, # confirmation
815  0, # param1 (0 to indicate disarm)
816  0, # param2 (all other params meaningless)
817  0, # param3
818  0, # param4
819  0, # param5
820  0, # param6
821  0) # param7
822 
823  def motors_armed(self):
824  '''return true if motors armed'''
825  return self.sysid_state[self.sysid].armed
826 
827  def motors_armed_wait(self):
828  '''wait for motors to be armed'''
829  while True:
830  m = self.wait_heartbeat()
831  if self.motors_armed():
832  return
833 
834  def motors_disarmed_wait(self):
835  '''wait for motors to be disarmed'''
836  while True:
837  m = self.wait_heartbeat()
838  if not self.motors_armed():
839  return
840 
841 
842  def field(self, type, field, default=None):
843  '''convenient function for returning an arbitrary MAVLink
844  field with a default'''
845  if not type in self.messages:
846  return default
847  return getattr(self.messages[type], field, default)
848 
849  def param(self, name, default=None):
850  '''convenient function for returning an arbitrary MAVLink
851  parameter with a default'''
852  if not name in self.params:
853  return default
854  return self.params[name]
855 
856  def setup_signing(self, secret_key, sign_outgoing=True, allow_unsigned_callback=None, initial_timestamp=None, link_id=None):
857  '''setup for MAVLink2 signing'''
858  self.mav.signing.secret_key = secret_key
859  self.mav.signing.sign_outgoing = sign_outgoing
860  self.mav.signing.allow_unsigned_callback = allow_unsigned_callback
861  if link_id is None:
862  # auto-increment the link_id for each link
863  global global_link_id
864  link_id = global_link_id
865  global_link_id = min(global_link_id + 1, 255)
866  self.mav.signing.link_id = link_id
867  if initial_timestamp is None:
868  # timestamp is time since 1/1/2015
869  epoch_offset = 1420070400
870  now = max(time.time(), epoch_offset)
871  initial_timestamp = now - epoch_offset
872  initial_timestamp = int(initial_timestamp * 100 * 1000)
873  # initial_timestamp is in 10usec units
874  self.mav.signing.timestamp = initial_timestamp
875 
876  def disable_signing(self):
877  '''disable MAVLink2 signing'''
878  self.mav.signing.secret_key = None
879  self.mav.signing.sign_outgoing = False
880  self.mav.signing.allow_unsigned_callback = None
881  self.mav.signing.link_id = 0
882  self.mav.signing.timestamp = 0
883 
884 def set_close_on_exec(fd):
885  '''set the clone on exec flag on a file descriptor. Ignore exceptions'''
886  try:
887  import fcntl
888  flags = fcntl.fcntl(fd, fcntl.F_GETFD)
889  flags |= fcntl.FD_CLOEXEC
890  fcntl.fcntl(fd, fcntl.F_SETFD, flags)
891  except Exception:
892  pass
893 
894 class FakeSerial():
895  def __init__(self):
896  pass
897  def read(self, len):
898  return ""
899  def write(self, buf):
900  raise Exception("write always fails")
901  def inWaiting(self):
902  return 0
903  def close(self):
904  pass
905 
906 class mavserial(mavfile):
907  '''a serial mavlink port'''
908  def __init__(self, device, baud=115200, autoreconnect=False, source_system=255, source_component=0, use_native=default_native, force_connected=False):
909  import serial
910  if ',' in device and not os.path.exists(device):
911  device, baud = device.split(',')
912  self.baud = baud
913  self.device = device
914  self.autoreconnect = autoreconnect
915  self.force_connected = force_connected
916  # we rather strangely set the baudrate initially to 1200, then change to the desired
917  # baudrate. This works around a kernel bug on some Linux kernels where the baudrate
918  # is not set correctly
919  try:
920  self.port = serial.Serial(self.device, 1200, timeout=0,
921  dsrdtr=False, rtscts=False, xonxoff=False)
922  except serial.SerialException as e:
923  if not force_connected:
924  raise e
925  self.port = FakeSerial()
926 
927  try:
928  fd = self.port.fileno()
929  set_close_on_exec(fd)
930  except Exception:
931  fd = None
932  self.set_baudrate(self.baud)
933  mavfile.__init__(self, fd, device, source_system=source_system, source_component=source_component, use_native=use_native)
934  self.rtscts = False
935 
936  def set_rtscts(self, enable):
937  '''enable/disable RTS/CTS if applicable'''
938  try:
939  self.port.setRtsCts(enable)
940  except Exception:
941  self.port.rtscts = enable
942  self.rtscts = enable
943 
944  def set_baudrate(self, baudrate):
945  '''set baudrate'''
946  try:
947  self.port.setBaudrate(baudrate)
948  except Exception:
949  # for pySerial 3.0, which doesn't have setBaudrate()
950  self.port.baudrate = baudrate
951 
952  def close(self):
953  self.port.close()
954 
955  def recv(self,n=None):
956  if n is None:
957  n = self.mav.bytes_needed()
958  if self.fd is None:
959  waiting = self.port.inWaiting()
960  if waiting < n:
961  n = waiting
962  ret = self.port.read(n)
963  return ret
964 
965  def write(self, buf):
966  try:
967  return self.port.write(bytes(buf))
968  except Exception:
969  if not self.portdead:
970  print("Device %s is dead" % self.device)
971  self.portdead = True
972  if self.autoreconnect:
973  self.reset()
974  return -1
975 
976  def reset(self):
977  import serial
978  try:
979  try:
980  newport = serial.Serial(self.device, self.baud, timeout=0,
981  dsrdtr=False, rtscts=False, xonxoff=False)
982  except serial.SerialException as e:
983  if not self.force_connected:
984  raise e
985  newport = FakeSerial()
986  return False
987  self.port.close()
988  self.port = newport
989  print("Device %s reopened OK" % self.device)
990  self.portdead = False
991  try:
992  self.fd = self.port.fileno()
993  except Exception:
994  self.fd = None
995  self.set_baudrate(self.baud)
996  if self.rtscts:
997  self.set_rtscts(self.rtscts)
998  return True
999  except Exception:
1000  return False
1001 
1002 
1003 class mavudp(mavfile):
1004  '''a UDP mavlink socket'''
1005  def __init__(self, device, input=True, broadcast=False, source_system=255, source_component=0, use_native=default_native):
1006  a = device.split(':')
1007  if len(a) != 2:
1008  print("UDP ports must be specified as host:port")
1009  sys.exit(1)
1010  self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
1011  self.udp_server = input
1012  self.broadcast = False
1013  if input:
1014  self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
1015  self.port.bind((a[0], int(a[1])))
1016  else:
1017  self.destination_addr = (a[0], int(a[1]))
1018  if broadcast:
1019  self.port.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
1020  self.broadcast = True
1021  set_close_on_exec(self.port.fileno())
1022  self.port.setblocking(0)
1023  self.last_address = None
1024  self.resolved_destination_addr = None
1025  mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, source_component=source_component, input=input, use_native=use_native)
1026 
1027  def close(self):
1028  self.port.close()
1029 
1030  def recv(self,n=None):
1031  try:
1032  data, new_addr = self.port.recvfrom(UDP_MAX_PACKET_LEN)
1033  except socket.error as e:
1034  if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
1035  return ""
1036  raise
1037  if self.udp_server or self.broadcast:
1038  self.last_address = new_addr
1039  return data
1040 
1041  def write(self, buf):
1042  try:
1043  if self.udp_server:
1044  if self.last_address:
1045  self.port.sendto(buf, self.last_address)
1046  else:
1047  if self.last_address and self.broadcast:
1048  self.destination_addr = self.last_address
1049  self.broadcast = False
1050  self.port.connect(self.destination_addr)
1051  # turn a (possible) hostname into an IP address to
1052  # avoid resolving the hostname for every packet sent:
1053  if self.destination_addr[0] != self.resolved_destination_addr:
1054  self.resolved_destination_addr = self.destination_addr[0]
1055  self.destination_addr = (socket.gethostbyname(self.destination_addr[0]), self.destination_addr[1])
1056  self.port.sendto(buf, self.destination_addr)
1057  except socket.error:
1058  pass
1059 
1060  def recv_msg(self):
1061  '''message receive routine for UDP link'''
1062  self.pre_message()
1063  s = self.recv()
1064  if len(s) > 0:
1065  if self.first_byte:
1066  self.auto_mavlink_version(s)
1067 
1068  m = self.mav.parse_char(s)
1069  if m is not None:
1070  self.post_message(m)
1071 
1072  return m
1073 
1074 class mavmcast(mavfile):
1075  '''a UDP multicast mavlink socket'''
1076  def __init__(self, device, broadcast=False, source_system=255, source_component=0, use_native=default_native):
1077  a = device.split(':')
1078  mcast_ip = "239.255.145.50"
1079  mcast_port = 14550
1080  if len(a) == 1 and len(a[0]) > 0:
1081  mcast_port = int(a[0])
1082  elif len(a) > 1:
1083  mcast_ip = a[0]
1084  mcast_port = int(a[1])
1085 
1086  # first the receiving socket. We use separate sending and receiving
1087  # sockets so we can use the port number of the sending socket to detect
1088  # packets from ourselves
1089  self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
1090  self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
1091  self.port.bind((mcast_ip, mcast_port))
1092  mreq = struct.pack("4sl", socket.inet_aton(mcast_ip), socket.INADDR_ANY)
1093  self.port.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
1094  self.port.setblocking(0)
1095  set_close_on_exec(self.port.fileno())
1096 
1097  # now the sending socket
1098  self.port_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
1099  self.port_out.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
1100  self.port_out.setblocking(0)
1101  self.port_out.connect((mcast_ip, mcast_port))
1102  set_close_on_exec(self.port_out.fileno())
1103  self.myport = None
1104 
1105  mavfile.__init__(self, self.port.fileno(), device,
1106  source_system=source_system, source_component=source_component,
1107  input=False, use_native=use_native)
1108 
1109  def close(self):
1110  self.port.close()
1111  self.port_out.close()
1112 
1113  def recv(self,n=None):
1114  try:
1115  data, new_addr = self.port.recvfrom(UDP_MAX_PACKET_LEN)
1116  if self.myport is None:
1117  try:
1118  (myaddr,self.myport) = self.port_out.getsockname()
1119  except Exception:
1120  pass
1121  except socket.error as e:
1122  if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
1123  return ""
1124  raise
1125  if self.myport == new_addr[1]:
1126  # data from ourselves, discard
1127  return ''
1128  return data
1129 
1130  def write(self, buf):
1131  try:
1132  self.port_out.send(buf)
1133  except socket.error as e:
1134  pass
1135 
1136  def recv_msg(self):
1137  '''message receive routine for UDP link'''
1138  self.pre_message()
1139  s = self.recv()
1140  if len(s) > 0:
1141  if self.first_byte:
1142  self.auto_mavlink_version(s)
1143 
1144  m = self.mav.parse_char(s)
1145  if m is not None:
1146  self.post_message(m)
1147 
1148  return m
1149 
1150 
1151 class mavtcp(mavfile):
1152  '''a TCP mavlink socket'''
1153  def __init__(self,
1154  device,
1155  autoreconnect=False,
1156  source_system=255,
1157  source_component=0,
1158  retries=3,
1159  use_native=default_native):
1160  a = device.split(':')
1161  if len(a) != 2:
1162  print("TCP ports must be specified as host:port")
1163  sys.exit(1)
1164  self.destination_addr = (a[0], int(a[1]))
1165 
1166  self.autoreconnect = autoreconnect
1167 
1168  self.retries = retries
1169  self.do_connect()
1170 
1171  mavfile.__init__(self, self.port.fileno(), "tcp:" + device, source_system=source_system, source_component=source_component, use_native=use_native)
1172 
1173  def do_connect(self):
1174  self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
1175  retries = self.retries
1176  if retries <= 0:
1177  # try to connect at least once:
1178  retries = 1
1179  while retries >= 0:
1180  retries -= 1
1181  try:
1182  self.port.connect(self.destination_addr)
1183  break
1184  except Exception as e:
1185  if retries == 0:
1186  raise e
1187  print(e, "sleeping")
1188  time.sleep(1)
1189  self.port.setblocking(0)
1190  set_close_on_exec(self.port.fileno())
1191  self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
1192 
1193  def close(self):
1194  self.port.close()
1195 
1196  def handle_eof(self):
1197  # EOF
1198  print("EOF on TCP socket")
1199  if self.autoreconnect:
1200  print("Attempting reconnect")
1201  if self.port is not None:
1202  self.port.close()
1203  self.port = None
1204  self.do_connect()
1205 
1206  def recv(self,n=None):
1207  if self.port is None:
1208  self.handle_eof()
1209  if n is None:
1210  n = self.mav.bytes_needed()
1211  try:
1212  data = self.port.recv(n)
1213  except socket.error as e:
1214  if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
1215  return ""
1216  raise
1217  if len(data) == 0:
1218  self.handle_eof()
1219 
1220  return data
1221 
1222  def write(self, buf):
1223  if self.port is None:
1224  self.do_connect()
1225  try:
1226  self.port.send(buf)
1227  except socket.error:
1228  pass
1229 
1230 
1231 class mavtcpin(mavfile):
1232  '''a TCP input mavlink socket'''
1233  def __init__(self, device, source_system=255, source_component=0, retries=3, use_native=default_native):
1234  a = device.split(':')
1235  if len(a) != 2:
1236  print("TCP ports must be specified as host:port")
1237  sys.exit(1)
1238  self.listen = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
1239  self.listen_addr = (a[0], int(a[1]))
1240  self.listen.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
1241  self.listen.bind(self.listen_addr)
1242  self.listen.listen(1)
1243  self.listen.setblocking(0)
1244  set_close_on_exec(self.listen.fileno())
1245  self.listen.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
1246  mavfile.__init__(self, self.listen.fileno(), "tcpin:" + device, source_system=source_system, source_component=source_component, use_native=use_native)
1247  self.port = None
1248 
1249  def close(self):
1250  self.listen.close()
1251 
1252  def recv(self,n=None):
1253  if not self.port:
1254  try:
1255  (self.port, addr) = self.listen.accept()
1256  except Exception:
1257  return ''
1258  self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
1259  self.port.setblocking(0)
1260  set_close_on_exec(self.port.fileno())
1261  self.fd = self.port.fileno()
1262 
1263  if n is None:
1264  n = self.mav.bytes_needed()
1265  try:
1266  data = self.port.recv(n)
1267  except socket.error as e:
1268  if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
1269  return ""
1270  self.port.close()
1271  self.port = None
1272  self.fd = self.listen.fileno()
1273  return ''
1274  return data
1275 
1276  def write(self, buf):
1277  if self.port is None:
1278  return
1279  try:
1280  self.port.send(buf)
1281  except socket.error as e:
1282  if e.errno in [ errno.EPIPE ]:
1283  self.port.close()
1284  self.port = None
1285  self.fd = self.listen.fileno()
1286  pass
1287 
1288 
1289 class mavlogfile(mavfile):
1290  '''a MAVLink logfile reader/writer'''
1291  def __init__(self, filename, planner_format=None,
1292  write=False, append=False,
1293  robust_parsing=True, notimestamps=False, source_system=255, source_component=0, use_native=default_native):
1294  self.filename = filename
1295  self.writeable = write
1296  self.robust_parsing = robust_parsing
1297  self.planner_format = planner_format
1298  self._two64 = math.pow(2.0, 63)
1299  mode = 'rb'
1300  if self.writeable:
1301  if append:
1302  mode = 'ab'
1303  else:
1304  mode = 'wb'
1305  self.f = open(filename, mode)
1306  self.filesize = os.path.getsize(filename)
1307  self.percent = 0
1308  mavfile.__init__(self, None, filename, source_system=source_system, source_component=source_component, notimestamps=notimestamps, use_native=use_native)
1309  if self.notimestamps:
1310  self._timestamp = 0
1311  else:
1312  self._timestamp = time.time()
1313  self.stop_on_EOF = True
1314  self._last_message = None
1315  self._last_timestamp = None
1316  self._link = 0
1317 
1318  def close(self):
1319  self.f.close()
1320 
1321  def recv(self,n=None):
1322  if n is None:
1323  n = self.mav.bytes_needed()
1324  return self.f.read(n)
1325 
1326  def write(self, buf):
1327  self.f.write(buf)
1328 
1329  def scan_timestamp(self, tbuf):
1330  '''scan forward looking in a tlog for a timestamp in a reasonable range'''
1331  while True:
1332  (tusec,) = struct.unpack('>Q', tbuf)
1333  t = tusec * 1.0e-6
1334  if abs(t - self._last_timestamp) <= 3*24*60*60:
1335  break
1336  c = self.f.read(1)
1337  if len(c) != 1:
1338  break
1339  tbuf = tbuf[1:] + c
1340  return t
1341 
1342 
1343  def pre_message(self):
1344  '''read timestamp if needed'''
1345  # read the timestamp
1346  if self.filesize != 0:
1347  self.percent = (100.0 * self.f.tell()) / self.filesize
1348  if self.notimestamps:
1349  return
1350  if self.planner_format:
1351  tbuf = self.f.read(21)
1352  if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':':
1353  raise RuntimeError('bad planner timestamp %s' % tbuf)
1354  hnsec = self._two64 + float(tbuf[0:20])
1355  t = hnsec * 1.0e-7 # convert to seconds
1356  t -= 719163 * 24 * 60 * 60 # convert to 1970 base
1357  self._link = 0
1358  else:
1359  tbuf = self.f.read(8)
1360  if len(tbuf) != 8:
1361  return
1362  (tusec,) = struct.unpack('>Q', tbuf)
1363  t = tusec * 1.0e-6
1364  if (self._last_timestamp is not None and
1365  self._last_message.get_type() == "BAD_DATA" and
1366  abs(t - self._last_timestamp) > 3*24*60*60):
1367  t = self.scan_timestamp(tbuf)
1368  self._link = tusec & 0x3
1369  self._timestamp = t
1370 
1371  def post_message(self, msg):
1372  '''add timestamp to message'''
1373  # read the timestamp
1374  super(mavlogfile, self).post_message(msg)
1375  if self.planner_format:
1376  self.f.read(1) # trailing newline
1377  self.timestamp = msg._timestamp
1378  self._last_message = msg
1379  if msg.get_type() != "BAD_DATA":
1380  self._last_timestamp = msg._timestamp
1381  msg._link = self._link
1382 
1383 
1384 class mavmmaplog(mavlogfile):
1385  '''a MAVLink log file accessed via mmap. Used for fast read-only
1386  access with low memory overhead where particular message types are wanted'''
1387  def __init__(self, filename, progress_callback=None):
1388  import platform, mmap
1389  mavlogfile.__init__(self, filename)
1390  self.f.seek(0, 2)
1391  self.data_len = self.f.tell()
1392  self.f.seek(0)
1393  if platform.system() == "Windows":
1394  self.data_map = mmap.mmap(self.f.fileno(), self.data_len, None, mmap.ACCESS_READ)
1395  else:
1396  self.data_map = mmap.mmap(self.f.fileno(), self.data_len, mmap.MAP_PRIVATE, mmap.PROT_READ)
1397  self._rewind()
1398  self.init_arrays(progress_callback)
1399  self._flightmodes = None
1400 
1401  def _rewind(self):
1402  '''rewind to start of log'''
1403  self.flightmode = "UNKNOWN"
1404  self.offset = 0
1405  self.type_nums = None
1406  self.f.seek(0)
1407 
1408  def rewind(self):
1409  '''rewind to start of log'''
1410  self._rewind()
1411 
1412  def init_arrays(self, progress_callback=None):
1413  '''initialise arrays for fast recv_match()'''
1414 
1415  # dictionary indexed by msgid, mapping to arrays of file offsets where
1416  # each instance of a msg type is found
1417  self.offsets = {}
1418 
1419  # number of msgs of each msg type
1420  self.counts = {}
1421  self._count = 0
1422 
1423  # mapping from msg name to msg id
1424  self.name_to_id = {}
1425 
1426  # mapping from msg id to name
1427  self.id_to_name = {}
1428 
1429  self.type_nums = None
1430 
1431  ofs = 0
1432  pct = 0
1433 
1434  MARKER_V1 = 0xFE
1435  MARKER_V2 = 0xFD
1436 
1437  while ofs+8+6 < self.data_len:
1438  marker = u_ord(self.data_map[ofs+8])
1439  mlen = u_ord(self.data_map[ofs+9]) + 8
1440  if marker == MARKER_V1:
1441  mtype = u_ord(self.data_map[ofs+13])
1442  mlen += 8
1443  elif marker == MARKER_V2:
1444  mtype = u_ord(self.data_map[ofs+15]) | (u_ord(self.data_map[ofs+16])<<8) | (u_ord(self.data_map[ofs+17])<<16)
1445  mlen += 12
1446  incompat_flags = u_ord(self.data_map[ofs+10])
1447  if incompat_flags & mavlink.MAVLINK_IFLAG_SIGNED:
1448  mlen += mavlink.MAVLINK_SIGNATURE_BLOCK_LEN
1449  else:
1450  # unrecognised marker; probably a malformed log
1451  ofs += 1
1452  continue
1453 
1454  if not mtype in self.offsets:
1455  if not mtype in mavlink.mavlink_map:
1456  ofs += mlen
1457  continue
1458  self.offsets[mtype] = []
1459  self.counts[mtype] = 0
1460  msg = mavlink.mavlink_map[mtype]
1461  self.name_to_id[msg.name] = mtype
1462  self.id_to_name[mtype] = msg.name
1463  self.f.seek(ofs)
1464  m = self.recv_msg()
1465  self.messages[msg.name] = m
1466 
1467  self.offsets[mtype].append(ofs)
1468  self.counts[mtype] += 1
1469 
1470  ofs += mlen
1471  new_pct = (100 * ofs) // self.data_len
1472  if progress_callback is not None and new_pct != pct:
1473  progress_callback(new_pct)
1474  pct = new_pct
1475 
1476  for mtype in self.counts:
1477  self._count += self.counts[mtype]
1478  self.offset = 0
1479  self._rewind()
1480 
1481  def skip_to_type(self, type):
1482  '''skip fwd to next msg matching given type set'''
1483  if self.type_nums is None:
1484  # always add some key msg types so we can track flightmode, params etc
1485  type = type.copy()
1486  type.update(set(['HEARTBEAT','PARAM_VALUE']))
1487  self.indexes = []
1488  self.type_nums = []
1489  for t in type:
1490  if not t in self.name_to_id:
1491  continue
1492  self.type_nums.append(self.name_to_id[t])
1493  self.indexes.append(0)
1494  smallest_index = -1
1495  smallest_offset = self.data_len
1496  for i in range(len(self.type_nums)):
1497  mtype = self.type_nums[i]
1498  if self.indexes[i] >= self.counts[mtype]:
1499  continue
1500  ofs = self.offsets[mtype][self.indexes[i]]
1501  if ofs < smallest_offset:
1502  smallest_offset = ofs
1503  smallest_index = i
1504  if smallest_index >= 0:
1505  self.indexes[smallest_index] += 1
1506  self.offset = smallest_offset
1507  self.f.seek(smallest_offset)
1508 
1509  def recv_match(self, condition=None, type=None, blocking=False, timeout=None):
1510  '''recv the next message that matches the given condition
1511  type can be a string or a list of strings'''
1512  if type is not None:
1513  if isinstance(type, str):
1514  type = set([type])
1515  elif isinstance(type, list):
1516  type = set(type)
1517  while True:
1518  if type is not None:
1519  self.skip_to_type(type)
1520  m = self.recv_msg()
1521  if m is None:
1522  if blocking:
1523  for hook in self.idle_hooks:
1524  hook(self)
1525  if timeout is None:
1526  self.select(0.05)
1527  else:
1528  self.select(timeout/2)
1529  continue
1530  return None
1531  if type is not None and not m.get_type() in type:
1532  continue
1533  if not evaluate_condition(condition, self.messages):
1534  continue
1535  return m
1536 
1537  def flightmode_list(self):
1538  '''return an array of tuples for all flightmodes in log. Tuple is (modestring, t0, t1)'''
1539  tstamp = None
1540  fmode = None
1541  if self._flightmodes is None:
1542  self._rewind()
1543  self._flightmodes = []
1544  types = set(['HEARTBEAT'])
1545  while True:
1546  m = self.recv_match(type=types)
1547  if m is None:
1548  break
1549  tstamp = m._timestamp
1550  if self.flightmode == fmode:
1551  continue
1552  if len(self._flightmodes) > 0:
1553  (mode, t0, t1) = self._flightmodes[-1]
1554  self._flightmodes[-1] = (mode, t0, tstamp)
1555  self._flightmodes.append((self.flightmode, tstamp, None))
1556  fmode = self.flightmode
1557  if tstamp is not None:
1558  (mode, t0, t1) = self._flightmodes[-1]
1559  self._flightmodes[-1] = (mode, t0, tstamp)
1560 
1561  self._rewind()
1562  return self._flightmodes
1563 
1564 class mavchildexec(mavfile):
1565  '''a MAVLink child processes reader/writer'''
1566  def __init__(self, filename, source_system=255, source_component=0, use_native=default_native):
1567  from subprocess import Popen, PIPE
1568  import fcntl
1569 
1570  self.filename = filename
1571  self.child = Popen(filename, shell=False, stdout=PIPE, stdin=PIPE, bufsize=0)
1572  self.fd = self.child.stdout.fileno()
1573 
1574  fl = fcntl.fcntl(self.fd, fcntl.F_GETFL)
1575  fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
1576 
1577  fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL)
1578  fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
1579 
1580  mavfile.__init__(self, self.fd, filename, source_system=source_system, source_component=source_component, use_native=use_native)
1581 
1582  def close(self):
1583  self.child.close()
1584 
1585  def recv(self,n=None):
1586  try:
1587  x = self.child.stdout.read(1)
1588  except Exception:
1589  return ''
1590  return x
1591 
1592  def write(self, buf):
1593  self.child.stdin.write(buf)
1594 
1595 
1596 def mavlink_connection(device, baud=115200, source_system=255, source_component=0,
1597  planner_format=None, write=False, append=False,
1598  robust_parsing=True, notimestamps=False, input=True,
1599  dialect=None, autoreconnect=False, zero_time_base=False,
1600  retries=3, use_native=default_native,
1601  force_connected=False, progress_callback=None):
1602  '''open a serial, UDP, TCP or file mavlink connection'''
1603  global mavfile_global
1604 
1605  if force_connected:
1606  # force_connected implies autoreconnect
1607  autoreconnect = True
1608 
1609  if dialect is not None:
1610  set_dialect(dialect)
1611  if device.startswith('tcp:'):
1612  return mavtcp(device[4:],
1613  autoreconnect=autoreconnect,
1614  source_system=source_system,
1615  source_component=source_component,
1616  retries=retries,
1617  use_native=use_native)
1618  if device.startswith('tcpin:'):
1619  return mavtcpin(device[6:], source_system=source_system, source_component=source_component, retries=retries, use_native=use_native)
1620  if device.startswith('udpin:'):
1621  return mavudp(device[6:], input=True, source_system=source_system, source_component=source_component, use_native=use_native)
1622  if device.startswith('udpout:'):
1623  return mavudp(device[7:], input=False, source_system=source_system, source_component=source_component, use_native=use_native)
1624  if device.startswith('udpbcast:'):
1625  return mavudp(device[9:], input=False, source_system=source_system, source_component=source_component, use_native=use_native, broadcast=True)
1626  # For legacy purposes we accept the following syntax and let the caller to specify direction
1627  if device.startswith('udp:'):
1628  return mavudp(device[4:], input=input, source_system=source_system, source_component=source_component, use_native=use_native)
1629  if device.startswith('mcast:'):
1630  return mavmcast(device[6:], source_system=source_system, source_component=source_component, use_native=use_native)
1631 
1632  if device.lower().endswith('.bin') or device.lower().endswith('.px4log'):
1633  # support dataflash logs
1634  from pymavlink import DFReader
1635  m = DFReader.DFReader_binary(device, zero_time_base=zero_time_base, progress_callback=progress_callback)
1636  mavfile_global = m
1637  return m
1638 
1639  if device.endswith('.log'):
1640  # support dataflash text logs
1641  from pymavlink import DFReader
1642  if DFReader.DFReader_is_text_log(device):
1643  m = DFReader.DFReader_text(device, zero_time_base=zero_time_base, progress_callback=progress_callback)
1644  mavfile_global = m
1645  return m
1646 
1647  # list of suffixes to prevent setting DOS paths as UDP sockets
1648  logsuffixes = ['mavlink', 'log', 'raw', 'tlog' ]
1649  suffix = device.split('.')[-1].lower()
1650  if device.find(':') != -1 and not suffix in logsuffixes:
1651  return mavudp(device, source_system=source_system, source_component=source_component, input=input, use_native=use_native)
1652  if os.path.isfile(device):
1653  if device.endswith(".elf") or device.find("/bin/") != -1:
1654  print("executing '%s'" % device)
1655  return mavchildexec(device, source_system=source_system, source_component=source_component, use_native=use_native)
1656  elif not write and not append and not notimestamps:
1657  return mavmmaplog(device, progress_callback=progress_callback)
1658  else:
1659  return mavlogfile(device, planner_format=planner_format, write=write,
1660  append=append, robust_parsing=robust_parsing, notimestamps=notimestamps,
1661  source_system=source_system, source_component=source_component, use_native=use_native)
1662  return mavserial(device,
1663  baud=baud,
1664  source_system=source_system,
1665  source_component=source_component,
1666  autoreconnect=autoreconnect,
1667  use_native=use_native,
1668  force_connected=force_connected)
1669 
1670 class periodic_event(object):
1671  '''a class for fixed frequency events'''
1672  def __init__(self, frequency):
1673  self.frequency = float(frequency)
1674  self.last_time = time.time()
1675 
1676  def force(self):
1677  '''force immediate triggering'''
1678  self.last_time = 0
1679 
1680  def trigger(self):
1681  '''return True if we should trigger now'''
1682  tnow = time.time()
1683 
1684  if tnow < self.last_time:
1685  print("Warning, time moved backwards. Restarting timer.")
1686  self.last_time = tnow
1687 
1688  if self.last_time + (1.0/self.frequency) <= tnow:
1689  self.last_time = tnow
1690  return True
1691  return False
1692 
1693 
1694 try:
1695  from curses import ascii
1696  have_ascii = True
1697 except:
1698  have_ascii = False
1699 
1700 def is_printable(c):
1701  '''see if a character is printable'''
1702  global have_ascii
1703  if have_ascii:
1704  return ascii.isprint(c)
1705  if isinstance(c, int):
1706  ic = c
1707  else:
1708  ic = ord(c)
1709  return ic >= 32 and ic <= 126
1710 
1711 def all_printable(buf):
1712  '''see if a string is all printable'''
1713  for c in buf:
1714  if not is_printable(c) and not c in ['\r', '\n', '\t'] and not c in [ord('\r'), ord('\n'), ord('\t')]:
1715  return False
1716  return True
1717 
1718 class SerialPort(object):
1719  '''auto-detected serial port'''
1720  def __init__(self, device, description=None, hwid=None):
1721  self.device = device
1722  self.description = description
1723  self.hwid = hwid
1724 
1725  def __str__(self):
1726  ret = self.device
1727  if self.description is not None:
1728  ret += " : " + self.description
1729  if self.hwid is not None:
1730  ret += " : " + self.hwid
1731  return ret
1732 
1733 def auto_detect_serial_win32(preferred_list=['*']):
1734  '''try to auto-detect serial ports on win32'''
1735  try:
1736  from serial.tools.list_ports_windows import comports
1737  list = sorted(comports())
1738  except:
1739  return []
1740  ret = []
1741  others = []
1742  for port, description, hwid in list:
1743  matches = False
1744  p = SerialPort(port, description=description, hwid=hwid)
1745  for preferred in preferred_list:
1746  if fnmatch.fnmatch(description, preferred) or fnmatch.fnmatch(hwid, preferred):
1747  matches = True
1748  if matches:
1749  ret.append(p)
1750  else:
1751  others.append(p)
1752  if len(ret) > 0:
1753  return ret
1754  # now the rest
1755  ret.extend(others)
1756  return ret
1757 
1758 
1759 
1760 
1761 def auto_detect_serial_unix(preferred_list=['*']):
1762  '''try to auto-detect serial ports on unix'''
1763  import glob
1764  glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')
1765  ret = []
1766  others = []
1767  # try preferred ones first
1768  for d in glist:
1769  matches = False
1770  for preferred in preferred_list:
1771  if fnmatch.fnmatch(d, preferred):
1772  matches = True
1773  if matches:
1774  ret.append(SerialPort(d))
1775  else:
1776  others.append(SerialPort(d))
1777  if len(ret) > 0:
1778  return ret
1779  ret.extend(others)
1780  return ret
1781 
1782 def auto_detect_serial(preferred_list=['*']):
1783  '''try to auto-detect serial port'''
1784  # see if
1785  if os.name == 'nt':
1786  return auto_detect_serial_win32(preferred_list=preferred_list)
1787  return auto_detect_serial_unix(preferred_list=preferred_list)
1788 
1789 def mode_string_v09(msg):
1790  '''mode string for 0.9 protocol'''
1791  mode = msg.mode
1792  nav_mode = msg.nav_mode
1793 
1794  MAV_MODE_UNINIT = 0
1795  MAV_MODE_MANUAL = 2
1796  MAV_MODE_GUIDED = 3
1797  MAV_MODE_AUTO = 4
1798  MAV_MODE_TEST1 = 5
1799  MAV_MODE_TEST2 = 6
1800  MAV_MODE_TEST3 = 7
1801 
1802  MAV_NAV_GROUNDED = 0
1803  MAV_NAV_LIFTOFF = 1
1804  MAV_NAV_HOLD = 2
1805  MAV_NAV_WAYPOINT = 3
1806  MAV_NAV_VECTOR = 4
1807  MAV_NAV_RETURNING = 5
1808  MAV_NAV_LANDING = 6
1809  MAV_NAV_LOST = 7
1810  MAV_NAV_LOITER = 8
1811 
1812  cmode = (mode, nav_mode)
1813  mapping = {
1814  (MAV_MODE_UNINIT, MAV_NAV_GROUNDED) : "INITIALISING",
1815  (MAV_MODE_MANUAL, MAV_NAV_VECTOR) : "MANUAL",
1816  (MAV_MODE_TEST3, MAV_NAV_VECTOR) : "CIRCLE",
1817  (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
1818  (MAV_MODE_TEST1, MAV_NAV_VECTOR) : "STABILIZE",
1819  (MAV_MODE_TEST2, MAV_NAV_LIFTOFF) : "FBWA",
1820  (MAV_MODE_AUTO, MAV_NAV_WAYPOINT) : "AUTO",
1821  (MAV_MODE_AUTO, MAV_NAV_RETURNING) : "RTL",
1822  (MAV_MODE_AUTO, MAV_NAV_LOITER) : "LOITER",
1823  (MAV_MODE_AUTO, MAV_NAV_LIFTOFF) : "TAKEOFF",
1824  (MAV_MODE_AUTO, MAV_NAV_LANDING) : "LANDING",
1825  (MAV_MODE_AUTO, MAV_NAV_HOLD) : "LOITER",
1826  (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
1827  (MAV_MODE_GUIDED, MAV_NAV_WAYPOINT) : "GUIDED",
1828  (100, MAV_NAV_VECTOR) : "STABILIZE",
1829  (101, MAV_NAV_VECTOR) : "ACRO",
1830  (102, MAV_NAV_VECTOR) : "ALT_HOLD",
1831  (107, MAV_NAV_VECTOR) : "CIRCLE",
1832  (109, MAV_NAV_VECTOR) : "LAND",
1833  }
1834  if cmode in mapping:
1835  return mapping[cmode]
1836  return "Mode(%s,%s)" % cmode
1837 
1838 mode_mapping_apm = {
1839  0 : 'MANUAL',
1840  1 : 'CIRCLE',
1841  2 : 'STABILIZE',
1842  3 : 'TRAINING',
1843  4 : 'ACRO',
1844  5 : 'FBWA',
1845  6 : 'FBWB',
1846  7 : 'CRUISE',
1847  8 : 'AUTOTUNE',
1848  10 : 'AUTO',
1849  11 : 'RTL',
1850  12 : 'LOITER',
1851  14 : 'LAND',
1852  15 : 'GUIDED',
1853  16 : 'INITIALISING',
1854  17 : 'QSTABILIZE',
1855  18 : 'QHOVER',
1856  19 : 'QLOITER',
1857  20 : 'QLAND',
1858  21 : 'QRTL',
1859  22 : 'QAUTOTUNE',
1860  }
1861 mode_mapping_acm = {
1862  0 : 'STABILIZE',
1863  1 : 'ACRO',
1864  2 : 'ALT_HOLD',
1865  3 : 'AUTO',
1866  4 : 'GUIDED',
1867  5 : 'LOITER',
1868  6 : 'RTL',
1869  7 : 'CIRCLE',
1870  8 : 'POSITION',
1871  9 : 'LAND',
1872  10 : 'OF_LOITER',
1873  11 : 'DRIFT',
1874  13 : 'SPORT',
1875  14 : 'FLIP',
1876  15 : 'AUTOTUNE',
1877  16 : 'POSHOLD',
1878  17 : 'BRAKE',
1879  18 : 'THROW',
1880  19 : 'AVOID_ADSB',
1881  20 : 'GUIDED_NOGPS',
1882  21 : 'SMART_RTL',
1883  22 : 'FLOWHOLD',
1884  23 : 'FOLLOW',
1885 }
1886 mode_mapping_rover = {
1887  0 : 'MANUAL',
1888  1 : 'ACRO',
1889  2 : 'LEARNING',
1890  3 : 'STEERING',
1891  4 : 'HOLD',
1892  5 : 'LOITER',
1893  10 : 'AUTO',
1894  11 : 'RTL',
1895  12 : 'SMART_RTL',
1896  15 : 'GUIDED',
1897  16 : 'INITIALISING'
1898  }
1899 
1900 mode_mapping_tracker = {
1901  0 : 'MANUAL',
1902  1 : 'STOP',
1903  2 : 'SCAN',
1904  10 : 'AUTO',
1905  16 : 'INITIALISING'
1906  }
1907 
1908 mode_mapping_sub = {
1909  0: 'STABILIZE',
1910  1: 'ACRO',
1911  2: 'ALT_HOLD',
1912  3: 'AUTO',
1913  4: 'GUIDED',
1914  7: 'CIRCLE',
1915  9: 'SURFACE',
1916  16: 'POSHOLD',
1917  19: 'MANUAL',
1918  }
1919 
1920 # map from a PX4 "main_state" to a string; see msg/commander_state.msg
1921 # This allows us to map sdlog STAT.MainState to a simple "mode"
1922 # string, used in DFReader and possibly other places. These are
1923 # related but distict from what is found in mavlink messages; see
1924 # "Custom mode definitions", below.
1925 mainstate_mapping_px4 = {
1926  0 : 'MANUAL',
1927  1 : 'ALTCTL',
1928  2 : 'POSCTL',
1929  3 : 'AUTO_MISSION',
1930  4 : 'AUTO_LOITER',
1931  5 : 'AUTO_RTL',
1932  6 : 'ACRO',
1933  7 : 'OFFBOARD',
1934  8 : 'STAB',
1935  9 : 'RATTITUDE',
1936  10 : 'AUTO_TAKEOFF',
1937  11 : 'AUTO_LAND',
1938  12 : 'AUTO_FOLLOW_TARGET',
1939  13 : 'MAX',
1940 }
1941 def mode_string_px4(MainState):
1942  return mainstate_mapping_px4.get(MainState, "Unknown")
1943 
1944 
1945 # Custom mode definitions from PX4
1946 PX4_CUSTOM_MAIN_MODE_MANUAL = 1
1947 PX4_CUSTOM_MAIN_MODE_ALTCTL = 2
1948 PX4_CUSTOM_MAIN_MODE_POSCTL = 3
1949 PX4_CUSTOM_MAIN_MODE_AUTO = 4
1950 PX4_CUSTOM_MAIN_MODE_ACRO = 5
1951 PX4_CUSTOM_MAIN_MODE_OFFBOARD = 6
1952 PX4_CUSTOM_MAIN_MODE_STABILIZED = 7
1953 PX4_CUSTOM_MAIN_MODE_RATTITUDE = 8
1954 
1955 PX4_CUSTOM_SUB_MODE_OFFBOARD = 0
1956 PX4_CUSTOM_SUB_MODE_AUTO_READY = 1
1957 PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF = 2
1958 PX4_CUSTOM_SUB_MODE_AUTO_LOITER = 3
1959 PX4_CUSTOM_SUB_MODE_AUTO_MISSION = 4
1960 PX4_CUSTOM_SUB_MODE_AUTO_RTL = 5
1961 PX4_CUSTOM_SUB_MODE_AUTO_LAND = 6
1962 PX4_CUSTOM_SUB_MODE_AUTO_RTGS = 7
1963 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET = 8
1964 
1965 auto_mode_flags = mavlink.MAV_MODE_FLAG_AUTO_ENABLED \
1966  | mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED \
1967  | mavlink.MAV_MODE_FLAG_GUIDED_ENABLED
1968 
1969 px4_map = { "MANUAL": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_MANUAL, 0 ),
1970  "STABILIZED": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_STABILIZED, 0 ),
1971  "ACRO": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_ACRO, 0 ),
1972  "RATTITUDE": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0 ),
1973  "ALTCTL": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_ALTCTL, 0 ),
1974  "POSCTL": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mavlink.MAV_MODE_FLAG_STABILIZE_ENABLED | mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, PX4_CUSTOM_MAIN_MODE_POSCTL, 0 ),
1975  "LOITER": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER ),
1976  "MISSION": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION ),
1977  "RTL": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL ),
1978  "LAND": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND ),
1979  "RTGS": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS ),
1980  "FOLLOWME": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET ),
1981  "OFFBOARD": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0 ),
1982  "TAKEOFF": (mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | auto_mode_flags, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF )}
1983 
1984 
1985 def interpret_px4_mode(base_mode, custom_mode):
1986  custom_main_mode = (custom_mode & 0xFF0000) >> 16
1987  custom_sub_mode = (custom_mode & 0xFF000000) >> 24
1988 
1989  if base_mode & mavlink.MAV_MODE_FLAG_MANUAL_INPUT_ENABLED != 0: #manual modes
1990  if custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL:
1991  return "MANUAL"
1992  elif custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO:
1993  return "ACRO"
1994  elif custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE:
1995  return "RATTITUDE"
1996  elif custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED:
1997  return "STABILIZED"
1998  elif custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL:
1999  return "ALTCTL"
2000  elif custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL:
2001  return "POSCTL"
2002  elif (base_mode & auto_mode_flags) == auto_mode_flags: #auto modes
2003  if custom_main_mode & PX4_CUSTOM_MAIN_MODE_AUTO != 0:
2004  if custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
2005  return "MISSION"
2006  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
2007  return "TAKEOFF"
2008  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
2009  return "LOITER"
2010  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
2011  return "FOLLOWME"
2012  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL:
2013  return "RTL"
2014  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND:
2015  return "LAND"
2016  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTGS:
2017  return "RTGS"
2018  elif custom_sub_mode == PX4_CUSTOM_SUB_MODE_OFFBOARD:
2019  return "OFFBOARD"
2020  return "UNKNOWN"
2021 
2022 def mode_mapping_byname(mav_type):
2023  '''return dictionary mapping mode names to numbers, or None if unknown'''
2024  map = None
2025  if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
2026  mavlink.MAV_TYPE_HELICOPTER,
2027  mavlink.MAV_TYPE_HEXAROTOR,
2028  mavlink.MAV_TYPE_OCTOROTOR,
2029  mavlink.MAV_TYPE_COAXIAL,
2030  mavlink.MAV_TYPE_TRICOPTER]:
2031  map = mode_mapping_acm
2032  if mav_type == mavlink.MAV_TYPE_FIXED_WING:
2033  map = mode_mapping_apm
2034  if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
2035  map = mode_mapping_rover
2036  if mav_type == mavlink.MAV_TYPE_SURFACE_BOAT:
2037  map = mode_mapping_rover # for the time being
2038  if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
2039  map = mode_mapping_tracker
2040  if mav_type == mavlink.MAV_TYPE_SUBMARINE:
2041  map = mode_mapping_sub
2042  if map is None:
2043  return None
2044  inv_map = dict((a, b) for (b, a) in map.items())
2045  return inv_map
2046 
2047 def mode_mapping_bynumber(mav_type):
2048  '''return dictionary mapping mode numbers to name, or None if unknown'''
2049  map = None
2050  if mav_type in [mavlink.MAV_TYPE_QUADROTOR,
2051  mavlink.MAV_TYPE_HELICOPTER,
2052  mavlink.MAV_TYPE_HEXAROTOR,
2053  mavlink.MAV_TYPE_OCTOROTOR,
2054  mavlink.MAV_TYPE_DODECAROTOR,
2055  mavlink.MAV_TYPE_COAXIAL,
2056  mavlink.MAV_TYPE_TRICOPTER]:
2057  map = mode_mapping_acm
2058  if mav_type == mavlink.MAV_TYPE_FIXED_WING:
2059  map = mode_mapping_apm
2060  if mav_type == mavlink.MAV_TYPE_GROUND_ROVER:
2061  map = mode_mapping_rover
2062  if mav_type == mavlink.MAV_TYPE_SURFACE_BOAT:
2063  map = mode_mapping_rover # for the time being
2064  if mav_type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
2065  map = mode_mapping_tracker
2066  if mav_type == mavlink.MAV_TYPE_SUBMARINE:
2067  map = mode_mapping_sub
2068  if map is None:
2069  return None
2070  return map
2071 
2072 
2073 def mode_string_v10(msg):
2074  '''mode string for 1.0 protocol, from heartbeat'''
2075  if msg.autopilot == mavlink.MAV_AUTOPILOT_PX4:
2076  return interpret_px4_mode(msg.base_mode, msg.custom_mode)
2077  if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
2078  return "Mode(0x%08x)" % msg.base_mode
2079  if msg.type in [ mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_TYPE_HEXAROTOR,
2080  mavlink.MAV_TYPE_OCTOROTOR, mavlink.MAV_TYPE_TRICOPTER,
2081  mavlink.MAV_TYPE_COAXIAL,
2082  mavlink.MAV_TYPE_HELICOPTER ]:
2083  if msg.custom_mode in mode_mapping_acm:
2084  return mode_mapping_acm[msg.custom_mode]
2085  if msg.type == mavlink.MAV_TYPE_FIXED_WING:
2086  if msg.custom_mode in mode_mapping_apm:
2087  return mode_mapping_apm[msg.custom_mode]
2088  if msg.type == mavlink.MAV_TYPE_GROUND_ROVER:
2089  if msg.custom_mode in mode_mapping_rover:
2090  return mode_mapping_rover[msg.custom_mode]
2091  if msg.type == mavlink.MAV_TYPE_SURFACE_BOAT:
2092  if msg.custom_mode in mode_mapping_rover:
2093  return mode_mapping_rover[msg.custom_mode]
2094  if msg.type == mavlink.MAV_TYPE_ANTENNA_TRACKER:
2095  if msg.custom_mode in mode_mapping_tracker:
2096  return mode_mapping_tracker[msg.custom_mode]
2097  if msg.type == mavlink.MAV_TYPE_SUBMARINE:
2098  if msg.custom_mode in mode_mapping_sub:
2099  return mode_mapping_sub[msg.custom_mode]
2100  return "Mode(%u)" % msg.custom_mode
2101 
2102 def mode_string_apm(mode_number):
2103  '''return mode string for APM:Plane'''
2104  if mode_number in mode_mapping_apm:
2105  return mode_mapping_apm[mode_number]
2106  return "Mode(%u)" % mode_number
2107 
2108 def mode_string_acm(mode_number):
2109  '''return mode string for APM:Copter'''
2110  if mode_number in mode_mapping_acm:
2111  return mode_mapping_acm[mode_number]
2112  return "Mode(%u)" % mode_number
2113 
2114 class x25crc(object):
2115  '''x25 CRC - based on checksum.h from mavlink library'''
2116  def __init__(self, buf=''):
2117  self.crc = 0xffff
2118  self.accumulate(buf)
2119 
2120  def accumulate(self, buf):
2121  '''add in some more bytes'''
2122  byte_buf = array.array('B')
2123  if isinstance(buf, array.array):
2124  byte_buf.extend(buf)
2125  else:
2126  byte_buf.fromstring(buf)
2127  accum = self.crc
2128  for b in byte_buf:
2129  tmp = b ^ (accum & 0xff)
2130  tmp = (tmp ^ (tmp<<4)) & 0xFF
2131  accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
2132  accum = accum & 0xFFFF
2133  self.crc = accum
2134 
2135 class MavlinkSerialPort(object):
2136  '''an object that looks like a serial port, but
2137  transmits using mavlink SERIAL_CONTROL packets'''
2138  def __init__(self, portname, baudrate, devnum=0, devbaud=0, timeout=3, debug=0):
2139  from . import mavutil
2140 
2141  self.baudrate = 0
2142  self.timeout = timeout
2143  self._debug = debug
2144  self.buf = ''
2145  self.port = devnum
2146  self.debug("Connecting with MAVLink to %s ..." % portname)
2147  self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
2148  self.mav.wait_heartbeat()
2149  self.debug("HEARTBEAT OK\n")
2150  if devbaud != 0:
2151  self.setBaudrate(devbaud)
2152  self.debug("Locked serial device\n")
2153 
2154  def debug(self, s, level=1):
2155  '''write some debug text'''
2156  if self._debug >= level:
2157  print(s)
2158 
2159  def write(self, b):
2160  '''write some bytes'''
2161  from . import mavutil
2162  self.debug("sending '%s' (0x%02x) of len %u\n" % (b, ord(b[0]), len(b)), 2)
2163  while len(b) > 0:
2164  n = len(b)
2165  if n > 70:
2166  n = 70
2167  buf = [ord(x) for x in b[:n]]
2168  buf.extend([0]*(70-len(buf)))
2169  self.mav.mav.serial_control_send(self.port,
2170  mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
2171  mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
2172  0,
2173  0,
2174  n,
2175  buf)
2176  b = b[n:]
2177 
2178  def _recv(self):
2179  '''read some bytes into self.buf'''
2180  from . import mavutil
2181  start_time = time.time()
2182  while time.time() < start_time + self.timeout:
2183  m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
2184  type='SERIAL_CONTROL', blocking=False, timeout=0)
2185  if m is not None and m.count != 0:
2186  break
2187  self.mav.mav.serial_control_send(self.port,
2188  mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
2189  mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
2190  0,
2191  0,
2192  0, [0]*70)
2193  m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
2194  type='SERIAL_CONTROL', blocking=True, timeout=0.01)
2195  if m is not None and m.count != 0:
2196  break
2197  if m is not None:
2198  if self._debug > 2:
2199  print(m)
2200  data = m.data[:m.count]
2201  self.buf += ''.join(str(chr(x)) for x in data)
2202 
2203  def read(self, n):
2204  '''read some bytes'''
2205  if len(self.buf) == 0:
2206  self._recv()
2207  if len(self.buf) > 0:
2208  if n > len(self.buf):
2209  n = len(self.buf)
2210  ret = self.buf[:n]
2211  self.buf = self.buf[n:]
2212  if self._debug >= 2:
2213  for b in ret:
2214  self.debug("read 0x%x" % ord(b), 2)
2215  return ret
2216  return ''
2217 
2218  def flushInput(self):
2219  '''flush any pending input'''
2220  self.buf = ''
2221  saved_timeout = self.timeout
2222  self.timeout = 0.5
2223  self._recv()
2224  self.timeout = saved_timeout
2225  self.buf = ''
2226  self.debug("flushInput")
2227 
2228  def setBaudrate(self, baudrate):
2229  '''set baudrate'''
2230  from . import mavutil
2231  if self.baudrate == baudrate:
2232  return
2233  self.baudrate = baudrate
2234  self.mav.mav.serial_control_send(self.port,
2235  mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE,
2236  0,
2237  self.baudrate,
2238  0, [0]*70)
2239  self.flushInput()
2240  self.debug("Changed baudrate %u" % self.baudrate)
2241 
2242 if __name__ == '__main__':
2243  serial_list = auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*'])
2244  for port in serial_list:
2245  print("%s" % port)
pymavlink.mavutil.periodic_event.force
def force(self)
Definition: mavutil.py:1676
pymavlink.mavutil.mavfile_state.mav_type
mav_type
Definition: mavutil.py:118
pymavlink.mavutil.periodic_event.frequency
frequency
Definition: mavutil.py:1673
pymavlink.mavutil.mavfile_state.vehicle_type
vehicle_type
Definition: mavutil.py:117
pymavlink.mavutil.mavfile_state.armed
armed
Definition: mavutil.py:120
pymavlink.DFReader.DFReader_text
Definition: DFReader.py:910
pymavlink.mavutil.param_state
Definition: mavutil.py:128
pymavlink.mavutil.periodic_event.trigger
def trigger(self)
Definition: mavutil.py:1680
pymavlink.mavutil.mavfile_state.__init__
def __init__(self)
Definition: mavutil.py:114
pymavlink.mavutil.periodic_event.last_time
last_time
Definition: mavutil.py:1674
pymavlink.mavutil.param_state.params
params
Definition: mavutil.py:131
pymavlink.mavutil.mavfile_state.base_mode
base_mode
Definition: mavutil.py:119
pymavlink.mavutil.param_state.__init__
def __init__(self)
Definition: mavutil.py:130
pymavlink.mavutil.mavfile_state.flightmode
flightmode
Definition: mavutil.py:116
pymavlink.mavutil.mavfile_state
Definition: mavutil.py:112
pymavlink.mavutil.mavfile_state.messages
messages
Definition: mavutil.py:115
pymavlink.mavutil.periodic_event
Definition: mavutil.py:1670
pymavlink.mavutil.periodic_event.__init__
def __init__(self, frequency)
Definition: mavutil.py:1672
pymavlink.DFReader.DFReader_binary
Definition: DFReader.py:645

mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07