aboutsummaryrefslogtreecommitdiff
path: root/mavlink/share/pyshared/pymavlink/mavutil.py
diff options
context:
space:
mode:
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/mavutil.py')
-rw-r--r--mavlink/share/pyshared/pymavlink/mavutil.py678
1 files changed, 678 insertions, 0 deletions
diff --git a/mavlink/share/pyshared/pymavlink/mavutil.py b/mavlink/share/pyshared/pymavlink/mavutil.py
new file mode 100644
index 000000000..c0f625214
--- /dev/null
+++ b/mavlink/share/pyshared/pymavlink/mavutil.py
@@ -0,0 +1,678 @@
+#!/usr/bin/env python
+'''
+mavlink python utility functions
+
+Copyright Andrew Tridgell 2011
+Released under GNU GPL version 3 or later
+'''
+
+import socket, math, struct, time, os, fnmatch, array, sys, errno
+from math import *
+from mavextra import *
+
+if os.getenv('MAVLINK10'):
+ import mavlinkv10 as mavlink
+else:
+ import mavlink
+
+def evaluate_expression(expression, vars):
+ '''evaluation an expression'''
+ try:
+ v = eval(expression, globals(), vars)
+ except NameError:
+ return None
+ return v
+
+def evaluate_condition(condition, vars):
+ '''evaluation a conditional (boolean) statement'''
+ if condition is None:
+ return True
+ v = evaluate_expression(condition, vars)
+ if v is None:
+ return False
+ return v
+
+
+class mavfile(object):
+ '''a generic mavlink port'''
+ def __init__(self, fd, address, source_system=255, notimestamps=False):
+ self.fd = fd
+ self.address = address
+ self.messages = { 'MAV' : self }
+ if mavlink.WIRE_PROTOCOL_VERSION == "1.0":
+ self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0)
+ mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message
+ else:
+ self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
+ self.params = {}
+ self.mav = None
+ self.target_system = 0
+ self.target_component = 0
+ self.mav = mavlink.MAVLink(self, srcSystem=source_system)
+ self.mav.robust_parsing = True
+ self.logfile = None
+ self.logfile_raw = None
+ self.param_fetch_in_progress = False
+ self.param_fetch_complete = False
+ self.start_time = time.time()
+ self.flightmode = "UNKNOWN"
+ self.timestamp = 0
+ self.message_hooks = []
+ self.idle_hooks = []
+ self.usec = 0
+ self.notimestamps = notimestamps
+ self._timestamp = None
+
+ def recv(self, n=None):
+ '''default recv method'''
+ raise RuntimeError('no recv() method supplied')
+
+ def close(self, n=None):
+ '''default close method'''
+ raise RuntimeError('no close() method supplied')
+
+ def write(self, buf):
+ '''default write method'''
+ raise RuntimeError('no write() method supplied')
+
+ def pre_message(self):
+ '''default pre message call'''
+ return
+
+ def post_message(self, msg):
+ '''default post message call'''
+ msg._timestamp = time.time()
+ type = msg.get_type()
+ self.messages[type] = msg
+
+ if self._timestamp is not None:
+ if self.notimestamps:
+ if 'usec' in msg.__dict__:
+ self.usec = msg.usec / 1.0e6
+ msg._timestamp = self.usec
+ else:
+ msg._timestamp = self._timestamp
+
+ self.timestamp = msg._timestamp
+ if type == 'HEARTBEAT':
+ self.target_system = msg.get_srcSystem()
+ self.target_component = msg.get_srcComponent()
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.flightmode = mode_string_v10(msg)
+ elif type == 'PARAM_VALUE':
+ self.params[str(msg.param_id)] = msg.param_value
+ if msg.param_index+1 == msg.param_count:
+ self.param_fetch_in_progress = False
+ self.param_fetch_complete = True
+ elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
+ self.flightmode = mode_string_v09(msg)
+ elif type == 'GPS_RAW':
+ if self.messages['HOME'].fix_type < 2:
+ self.messages['HOME'] = msg
+ for hook in self.message_hooks:
+ hook(self, msg)
+
+
+ def recv_msg(self):
+ '''message receive routine'''
+ self.pre_message()
+ while True:
+ n = self.mav.bytes_needed()
+ s = self.recv(n)
+ if len(s) == 0 and len(self.mav.buf) == 0:
+ return None
+ if self.logfile_raw:
+ self.logfile_raw.write(str(s))
+ msg = self.mav.parse_char(s)
+ if msg:
+ self.post_message(msg)
+ return msg
+
+ def recv_match(self, condition=None, type=None, blocking=False):
+ '''recv the next MAVLink message that matches the given condition'''
+ while True:
+ m = self.recv_msg()
+ if m is None:
+ if blocking:
+ for hook in self.idle_hooks:
+ hook(self)
+ time.sleep(0.01)
+ continue
+ return None
+ if type is not None and type != m.get_type():
+ continue
+ if not evaluate_condition(condition, self.messages):
+ continue
+ return m
+
+ def setup_logfile(self, logfile, mode='w'):
+ '''start logging to the given logfile, with timestamps'''
+ self.logfile = open(logfile, mode=mode)
+
+ def setup_logfile_raw(self, logfile, mode='w'):
+ '''start logging raw bytes to the given logfile, without timestamps'''
+ self.logfile_raw = open(logfile, mode=mode)
+
+ def wait_heartbeat(self, blocking=True):
+ '''wait for a heartbeat so we know the target system IDs'''
+ return self.recv_match(type='HEARTBEAT', blocking=blocking)
+
+ def param_fetch_all(self):
+ '''initiate fetch of all parameters'''
+ if time.time() - getattr(self, 'param_fetch_start', 0) < 2.0:
+ # don't fetch too often
+ return
+ self.param_fetch_start = time.time()
+ self.param_fetch_in_progress = True
+ self.mav.param_request_list_send(self.target_system, self.target_component)
+
+ def time_since(self, mtype):
+ '''return the time since the last message of type mtype was received'''
+ if not mtype in self.messages:
+ return time.time() - self.start_time
+ return time.time() - self.messages[mtype]._timestamp
+
+ def param_set_send(self, parm_name, parm_value, parm_type=None):
+ '''wrapper for parameter set'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ if parm_type == None:
+ parm_type = mavlink.MAV_VAR_FLOAT
+ self.mav.param_set_send(self.target_system, self.target_component,
+ parm_name, parm_value, parm_type)
+ else:
+ self.mav.param_set_send(self.target_system, self.target_component,
+ parm_name, parm_value)
+
+ def waypoint_request_list_send(self):
+ '''wrapper for waypoint_request_list_send'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.mav.mission_request_list_send(self.target_system, self.target_component)
+ else:
+ self.mav.waypoint_request_list_send(self.target_system, self.target_component)
+
+ def waypoint_clear_all_send(self):
+ '''wrapper for waypoint_clear_all_send'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.mav.mission_clear_all_send(self.target_system, self.target_component)
+ else:
+ self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
+
+ def waypoint_request_send(self, seq):
+ '''wrapper for waypoint_request_send'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.mav.mission_request_send(self.target_system, self.target_component, seq)
+ else:
+ self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
+
+ def waypoint_set_current_send(self, seq):
+ '''wrapper for waypoint_set_current_send'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
+ else:
+ self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
+
+ def waypoint_count_send(self, seq):
+ '''wrapper for waypoint_count_send'''
+ if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+ self.mav.mission_count_send(self.target_system, self.target_component, seq)
+ else:
+ self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
+
+
+class mavserial(mavfile):
+ '''a serial mavlink port'''
+ def __init__(self, device, baud=115200, autoreconnect=False, source_system=255):
+ import serial
+ self.baud = baud
+ self.device = device
+ self.autoreconnect = autoreconnect
+ self.port = serial.Serial(self.device, self.baud, timeout=0,
+ dsrdtr=False, rtscts=False, xonxoff=False)
+
+ try:
+ fd = self.port.fileno()
+ except Exception:
+ fd = None
+ mavfile.__init__(self, fd, device, source_system=source_system)
+
+ def close(self):
+ self.port.close()
+
+ def recv(self,n=None):
+ if n is None:
+ n = self.mav.bytes_needed()
+ if self.fd is None:
+ waiting = self.port.inWaiting()
+ if waiting < n:
+ n = waiting
+ return self.port.read(n)
+
+ def write(self, buf):
+ try:
+ return self.port.write(buf)
+ except OSError:
+ if self.autoreconnect:
+ self.reset()
+ return -1
+
+ def reset(self):
+ import serial
+ self.port.close()
+ while True:
+ try:
+ self.port = serial.Serial(self.device, self.baud, timeout=1,
+ dsrdtr=False, rtscts=False, xonxoff=False)
+ try:
+ self.fd = self.port.fileno()
+ except Exception:
+ self.fd = None
+ return
+ except Exception:
+ print("Failed to reopen %s" % self.device)
+ time.sleep(1)
+
+
+class mavudp(mavfile):
+ '''a UDP mavlink socket'''
+ def __init__(self, device, input=True, source_system=255):
+ a = device.split(':')
+ if len(a) != 2:
+ print("UDP ports must be specified as host:port")
+ sys.exit(1)
+ self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self.udp_server = input
+ if input:
+ self.port.bind((a[0], int(a[1])))
+ else:
+ self.destination_addr = (a[0], int(a[1]))
+ self.port.setblocking(0)
+ self.last_address = None
+ mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
+
+ def close(self):
+ self.port.close()
+
+ def recv(self,n=None):
+ try:
+ data, self.last_address = self.port.recvfrom(300)
+ except socket.error as e:
+ if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
+ return ""
+ raise
+ return data
+
+ def write(self, buf):
+ try:
+ if self.udp_server:
+ if self.last_address:
+ self.port.sendto(buf, self.last_address)
+ else:
+ self.port.sendto(buf, self.destination_addr)
+ except socket.error:
+ pass
+
+ def recv_msg(self):
+ '''message receive routine for UDP link'''
+ self.pre_message()
+ s = self.recv()
+ if len(s) == 0:
+ return None
+ msg = self.mav.parse_buffer(s)
+ if msg is not None:
+ for m in msg:
+ self.post_message(m)
+ return msg[0]
+ return None
+
+
+class mavtcp(mavfile):
+ '''a TCP mavlink socket'''
+ def __init__(self, device, source_system=255):
+ a = device.split(':')
+ if len(a) != 2:
+ print("TCP ports must be specified as host:port")
+ sys.exit(1)
+ self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.destination_addr = (a[0], int(a[1]))
+ self.port.connect(self.destination_addr)
+ self.port.setblocking(0)
+ self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
+ mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
+
+ def close(self):
+ self.port.close()
+
+ def recv(self,n=None):
+ if n is None:
+ n = self.mav.bytes_needed()
+ try:
+ data = self.port.recv(n)
+ except socket.error as e:
+ if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
+ return ""
+ raise
+ return data
+
+ def write(self, buf):
+ try:
+ self.port.send(buf)
+ except socket.error:
+ pass
+
+
+class mavlogfile(mavfile):
+ '''a MAVLink logfile reader/writer'''
+ def __init__(self, filename, planner_format=None,
+ write=False, append=False,
+ robust_parsing=True, notimestamps=False, source_system=255):
+ self.filename = filename
+ self.writeable = write
+ self.robust_parsing = robust_parsing
+ self.planner_format = planner_format
+ self._two64 = math.pow(2.0, 63)
+ mode = 'rb'
+ if self.writeable:
+ if append:
+ mode = 'ab'
+ else:
+ mode = 'wb'
+ self.f = open(filename, mode)
+ self.filesize = os.path.getsize(filename)
+ self.percent = 0
+ mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps)
+ if self.notimestamps:
+ self._timestamp = 0
+ else:
+ self._timestamp = time.time()
+
+ def close(self):
+ self.f.close()
+
+ def recv(self,n=None):
+ if n is None:
+ n = self.mav.bytes_needed()
+ return self.f.read(n)
+
+ def write(self, buf):
+ self.f.write(buf)
+
+ def pre_message(self):
+ '''read timestamp if needed'''
+ # read the timestamp
+ self.percent = (100.0 * self.f.tell()) / self.filesize
+ if self.notimestamps:
+ return
+ if self.planner_format:
+ tbuf = self.f.read(21)
+ if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':':
+ raise RuntimeError('bad planner timestamp %s' % tbuf)
+ hnsec = self._two64 + float(tbuf[0:20])
+ t = hnsec * 1.0e-7 # convert to seconds
+ t -= 719163 * 24 * 60 * 60 # convert to 1970 base
+ else:
+ tbuf = self.f.read(8)
+ if len(tbuf) != 8:
+ return
+ (tusec,) = struct.unpack('>Q', tbuf)
+ t = tusec * 1.0e-6
+ self._timestamp = t
+
+ def post_message(self, msg):
+ '''add timestamp to message'''
+ # read the timestamp
+ super(mavlogfile, self).post_message(msg)
+ if self.planner_format:
+ self.f.read(1) # trailing newline
+ self.timestamp = msg._timestamp
+
+class mavchildexec(mavfile):
+ '''a MAVLink child processes reader/writer'''
+ def __init__(self, filename, source_system=255):
+ from subprocess import Popen, PIPE
+ import fcntl
+
+ self.filename = filename
+ self.child = Popen(filename, shell=True, stdout=PIPE, stdin=PIPE)
+ self.fd = self.child.stdout.fileno()
+
+ fl = fcntl.fcntl(self.fd, fcntl.F_GETFL)
+ fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
+
+ fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL)
+ fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
+
+ mavfile.__init__(self, self.fd, filename, source_system=source_system)
+
+ def close(self):
+ self.child.close()
+
+ def recv(self,n=None):
+ try:
+ x = self.child.stdout.read(1)
+ except Exception:
+ return ''
+ return x
+
+ def write(self, buf):
+ self.child.stdin.write(buf)
+
+
+def mavlink_connection(device, baud=115200, source_system=255,
+ planner_format=None, write=False, append=False,
+ robust_parsing=True, notimestamps=False, input=True):
+ '''make a serial or UDP mavlink connection'''
+ if device.startswith('tcp:'):
+ return mavtcp(device[4:], source_system=source_system)
+ if device.startswith('udp:'):
+ return mavudp(device[4:], input=input, source_system=source_system)
+ if device.find(':') != -1 and not device.endswith('log'):
+ return mavudp(device, source_system=source_system, input=input)
+ if os.path.isfile(device):
+ if device.endswith(".elf"):
+ return mavchildexec(device, source_system=source_system)
+ else:
+ return mavlogfile(device, planner_format=planner_format, write=write,
+ append=append, robust_parsing=robust_parsing, notimestamps=notimestamps,
+ source_system=source_system)
+ return mavserial(device, baud=baud, source_system=source_system)
+
+class periodic_event(object):
+ '''a class for fixed frequency events'''
+ def __init__(self, frequency):
+ self.frequency = float(frequency)
+ self.last_time = time.time()
+
+ def force(self):
+ '''force immediate triggering'''
+ self.last_time = 0
+
+ def trigger(self):
+ '''return True if we should trigger now'''
+ tnow = time.time()
+ if self.last_time + (1.0/self.frequency) <= tnow:
+ self.last_time = tnow
+ return True
+ return False
+
+
+try:
+ from curses import ascii
+ have_ascii = True
+except:
+ have_ascii = False
+
+def is_printable(c):
+ '''see if a character is printable'''
+ global have_ascii
+ if have_ascii:
+ return ascii.isprint(c)
+ if isinstance(c, int):
+ ic = c
+ else:
+ ic = ord(c)
+ return ic >= 32 and ic <= 126
+
+def all_printable(buf):
+ '''see if a string is all printable'''
+ for c in buf:
+ if not is_printable(c) and not c in ['\r', '\n', '\t']:
+ return False
+ return True
+
+class SerialPort(object):
+ '''auto-detected serial port'''
+ def __init__(self, device, description=None, hwid=None):
+ self.device = device
+ self.description = description
+ self.hwid = hwid
+
+ def __str__(self):
+ ret = self.device
+ if self.description is not None:
+ ret += " : " + self.description
+ if self.hwid is not None:
+ ret += " : " + self.hwid
+ return ret
+
+def auto_detect_serial_win32(preferred_list=['*']):
+ '''try to auto-detect serial ports on win32'''
+ try:
+ import scanwin32
+ list = sorted(scanwin32.comports())
+ except:
+ return []
+ ret = []
+ for order, port, desc, hwid in list:
+ for preferred in preferred_list:
+ if fnmatch.fnmatch(desc, preferred) or fnmatch.fnmatch(hwid, preferred):
+ ret.append(SerialPort(port, description=desc, hwid=hwid))
+ if len(ret) > 0:
+ return ret
+ # now the rest
+ for order, port, desc, hwid in list:
+ ret.append(SerialPort(port, description=desc, hwid=hwid))
+ return ret
+
+
+
+
+def auto_detect_serial_unix(preferred_list=['*']):
+ '''try to auto-detect serial ports on win32'''
+ import glob
+ glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')
+ ret = []
+ # try preferred ones first
+ for d in glist:
+ for preferred in preferred_list:
+ if fnmatch.fnmatch(d, preferred):
+ ret.append(SerialPort(d))
+ if len(ret) > 0:
+ return ret
+ # now the rest
+ for d in glist:
+ ret.append(SerialPort(d))
+ return ret
+
+
+
+def auto_detect_serial(preferred_list=['*']):
+ '''try to auto-detect serial port'''
+ # see if
+ if os.name == 'nt':
+ return auto_detect_serial_win32(preferred_list=preferred_list)
+ return auto_detect_serial_unix(preferred_list=preferred_list)
+
+def mode_string_v09(msg):
+ '''mode string for 0.9 protocol'''
+ mode = msg.mode
+ nav_mode = msg.nav_mode
+
+ MAV_MODE_UNINIT = 0
+ MAV_MODE_MANUAL = 2
+ MAV_MODE_GUIDED = 3
+ MAV_MODE_AUTO = 4
+ MAV_MODE_TEST1 = 5
+ MAV_MODE_TEST2 = 6
+ MAV_MODE_TEST3 = 7
+
+ MAV_NAV_GROUNDED = 0
+ MAV_NAV_LIFTOFF = 1
+ MAV_NAV_HOLD = 2
+ MAV_NAV_WAYPOINT = 3
+ MAV_NAV_VECTOR = 4
+ MAV_NAV_RETURNING = 5
+ MAV_NAV_LANDING = 6
+ MAV_NAV_LOST = 7
+ MAV_NAV_LOITER = 8
+
+ cmode = (mode, nav_mode)
+ mapping = {
+ (MAV_MODE_UNINIT, MAV_NAV_GROUNDED) : "INITIALISING",
+ (MAV_MODE_MANUAL, MAV_NAV_VECTOR) : "MANUAL",
+ (MAV_MODE_TEST3, MAV_NAV_VECTOR) : "CIRCLE",
+ (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
+ (MAV_MODE_TEST1, MAV_NAV_VECTOR) : "STABILIZE",
+ (MAV_MODE_TEST2, MAV_NAV_LIFTOFF) : "FBWA",
+ (MAV_MODE_AUTO, MAV_NAV_WAYPOINT) : "AUTO",
+ (MAV_MODE_AUTO, MAV_NAV_RETURNING) : "RTL",
+ (MAV_MODE_AUTO, MAV_NAV_LOITER) : "LOITER",
+ (MAV_MODE_AUTO, MAV_NAV_LIFTOFF) : "TAKEOFF",
+ (MAV_MODE_AUTO, MAV_NAV_LANDING) : "LANDING",
+ (MAV_MODE_AUTO, MAV_NAV_HOLD) : "LOITER",
+ (MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
+ (MAV_MODE_GUIDED, MAV_NAV_WAYPOINT) : "GUIDED",
+ (100, MAV_NAV_VECTOR) : "STABILIZE",
+ (101, MAV_NAV_VECTOR) : "ACRO",
+ (102, MAV_NAV_VECTOR) : "ALT_HOLD",
+ (107, MAV_NAV_VECTOR) : "CIRCLE",
+ (109, MAV_NAV_VECTOR) : "LAND",
+ }
+ if cmode in mapping:
+ return mapping[cmode]
+ return "Mode(%s,%s)" % cmode
+
+def mode_string_v10(msg):
+ '''mode string for 1.0 protocol, from heartbeat'''
+ if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
+ return "Mode(0x%08x)" % msg.base_mode
+ mapping = {
+ 0 : 'MANUAL',
+ 1 : 'CIRCLE',
+ 2 : 'STABILIZE',
+ 5 : 'FBWA',
+ 6 : 'FBWB',
+ 7 : 'FBWC',
+ 10 : 'AUTO',
+ 11 : 'RTL',
+ 12 : 'LOITER',
+ 13 : 'TAKEOFF',
+ 14 : 'LAND',
+ 15 : 'GUIDED',
+ 16 : 'INITIALISING'
+ }
+ if msg.custom_mode in mapping:
+ return mapping[msg.custom_mode]
+ return "Mode(%u)" % msg.custom_mode
+
+
+
+class x25crc(object):
+ '''x25 CRC - based on checksum.h from mavlink library'''
+ def __init__(self, buf=''):
+ self.crc = 0xffff
+ self.accumulate(buf)
+
+ def accumulate(self, buf):
+ '''add in some more bytes'''
+ bytes = array.array('B')
+ if isinstance(buf, array.array):
+ bytes.extend(buf)
+ else:
+ bytes.fromstring(buf)
+ accum = self.crc
+ for b in bytes:
+ tmp = b ^ (accum & 0xff)
+ tmp = (tmp ^ (tmp<<4)) & 0xFF
+ accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
+ accum = accum & 0xFFFF
+ self.crc = accum
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-31 15:55:17 +0000