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

mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02