mavlink: added auto-detection of mavlink protocol version

Tools/autotest/autotest.py

@@ -103,16 +103,9 @@ parser.add_option("--skip", type='string', default='', help='list of steps to sk
 parser.add_option("--list", action='store_true', default=False, help='list the available steps')
 parser.add_option("--viewerip", default=None, help='IP address to send MAVLink and fg packets to')
 parser.add_option("--experimental", default=False, action='store_true', help='enable experimental tests')
-parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9")
 
 opts, args = parser.parse_args()
 
-if not opts.mav09:
-    os.environ['MAVLINK10'] = '1'
-    import mavlinkv10 as mavlink
-else:
-    import mavlink as mavlink
-
 import  arducopter, arduplane
 
 steps = [

Tools/autotest/pymavlink/mavextra.py

@@ -17,10 +17,10 @@ def altitude(SCALED_PRESSURE):
     '''calculate barometric altitude'''
     import mavutil
     self = mavutil.mavfile_global
-    if self.ground_pressure is None or self.ground_temperature is None:
+    if self.param('GND_ABS_PRESS', None) is None:
         return 0
-    scaling = self.ground_pressure / (SCALED_PRESSURE.press_abs*100.0)
+    scaling = self.param('GND_ABS_PRESS', 1) / (SCALED_PRESSURE.press_abs*100.0)
-    temp = self.ground_temperature + 273.15
+    temp = self.param('GND_TEMP', 0) + 273.15
     return log(scaling) * temp * 29271.267 * 0.001
 
 

Tools/autotest/pymavlink/mavlink.py

@@ -10,6 +10,21 @@ import struct, array, mavutil, time
 
 WIRE_PROTOCOL_VERSION = "0.9"
 
+
+# some base types from mavlink_types.h
+MAVLINK_TYPE_CHAR     = 0
+MAVLINK_TYPE_UINT8_T  = 1
+MAVLINK_TYPE_INT8_T   = 2
+MAVLINK_TYPE_UINT16_T = 3
+MAVLINK_TYPE_INT16_T  = 4
+MAVLINK_TYPE_UINT32_T = 5
+MAVLINK_TYPE_INT32_T  = 6
+MAVLINK_TYPE_UINT64_T = 7
+MAVLINK_TYPE_INT64_T  = 8
+MAVLINK_TYPE_FLOAT    = 9
+MAVLINK_TYPE_DOUBLE   = 10
+
+
 class MAVLink_header(object):
     '''MAVLink message header'''
     def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
@@ -34,7 +49,9 @@ class MAVLink_message(object):
         self._type       = name
 
     def get_msgbuf(self):
-        return self._msgbuf
+        if isinstance(self._msgbuf, str):
+            return self._msgbuf
+        return self._msgbuf.tostring()
 
     def get_header(self):
         return self._header
@@ -2510,7 +2527,7 @@ class MAVLink(object):
                 msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)
                 msg.pack(self)
                 return msg
-
+            
         def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw):
                 '''
                 Status of DCM attitude estimator
@@ -2525,7 +2542,7 @@ class MAVLink(object):
 
                 '''
                 return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw))
-
+            
         def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
                 '''
                 Status of simulation environment, if used
@@ -2544,7 +2561,7 @@ class MAVLink(object):
                 msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)
                 msg.pack(self)
                 return msg
-
+            
         def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro):
                 '''
                 Status of simulation environment, if used
@@ -2561,7 +2578,7 @@ class MAVLink(object):
 
                 '''
                 return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro))
-
+            
         def hwstatus_encode(self, Vcc, I2Cerr):
                 '''
                 Status of key hardware
@@ -2573,7 +2590,7 @@ class MAVLink(object):
                 msg = MAVLink_hwstatus_message(Vcc, I2Cerr)
                 msg.pack(self)
                 return msg
-
+            
         def hwstatus_send(self, Vcc, I2Cerr):
                 '''
                 Status of key hardware
@@ -2583,7 +2600,7 @@ class MAVLink(object):
 
                 '''
                 return self.send(self.hwstatus_encode(Vcc, I2Cerr))
-
+            
         def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
                 '''
                 Status generated by radio
@@ -2600,7 +2617,7 @@ class MAVLink(object):
                 msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)
                 msg.pack(self)
                 return msg
-
+            
         def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed):
                 '''
                 Status generated by radio
@@ -2615,7 +2632,7 @@ class MAVLink(object):
 
                 '''
                 return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed))
-
+            
         def heartbeat_encode(self, type, autopilot, mavlink_version=2):
                 '''
                 The heartbeat message shows that a system is present and responding.

Tools/autotest/pymavlink/mavutil.py

@@ -6,7 +6,7 @@ Copyright Andrew Tridgell 2011
 Released under GNU GPL version 3 or later
 '''
 
-import socket, math, struct, time, os, fnmatch, array, sys, errno
+import socket, math, struct, time, os, fnmatch, array, sys, errno, fcntl
 from math import *
 from mavextra import *
 
@@ -47,9 +47,10 @@ class location(object):
 
 class mavfile(object):
     '''a generic mavlink port'''
-    def __init__(self, fd, address, source_system=255, notimestamps=False):
+    def __init__(self, fd, address, source_system=255, notimestamps=False, input=True):
         global mavfile_global
-        mavfile_global = self
+        if input:
+            mavfile_global = self
         self.fd = fd
         self.address = address
         self.messages = { 'MAV' : self }
@@ -59,11 +60,13 @@ class mavfile(object):
         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.source_system = source_system
-        self.mav.robust_parsing = True
+        self.first_byte = True
+        self.robust_parsing = True
+        self.mav = mavlink.MAVLink(self, srcSystem=self.source_system)
+        self.mav.robust_parsing = self.robust_parsing
         self.logfile = None
         self.logfile_raw = None
         self.param_fetch_in_progress = False
@@ -80,6 +83,36 @@ class mavfile(object):
         self.ground_temperature = None
         self.altitude = 0
         self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
+        self.last_seq = -1
+        self.mav_loss = 0
+        self.mav_count = 0
+        self.stop_on_EOF = False
+
+    def auto_mavlink_version(self, buf):
+        '''auto-switch mavlink protocol version'''
+        global mavlink
+        if len(buf) == 0:
+            return
+        if not ord(buf[0]) in [ 85, 254 ]:
+            return
+        self.first_byte = False
+        if self.WIRE_PROTOCOL_VERSION == "0.9" and ord(buf[0]) == 254:
+            import mavlinkv10 as mavlink
+            os.environ['MAVLINK10'] = '1'
+        elif self.WIRE_PROTOCOL_VERSION == "1.0" and ord(buf[0]) == 85:
+            import mavlink as mavlink
+        else:
+            return
+        # switch protocol 
+        (callback, callback_args, callback_kwargs) = (self.mav.callback,
+                                                      self.mav.callback_args,
+                                                      self.mav.callback_kwargs)
+        self.mav = mavlink.MAVLink(self, srcSystem=self.source_system)
+        self.mav.robust_parsing = self.robust_parsing
+        self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION
+        (self.mav.callback, self.mav.callback_args, self.mav.callback_kwargs) = (callback,
+                                                                                 callback_args,
+                                                                                 callback_kwargs)
 
     def recv(self, n=None):
         '''default recv method'''
@@ -99,6 +132,9 @@ class mavfile(object):
 
     def post_message(self, msg):
         '''default post message call'''
+        if '_posted' in msg.__dict__:
+            return
+        msg._posted = True
         msg._timestamp = time.time()
         type = msg.get_type()
         self.messages[type] = msg
@@ -112,6 +148,19 @@ class mavfile(object):
             else:
                 msg._timestamp = self._timestamp
 
+        if not (
+            # its the radio or planner
+            (msg.get_srcSystem() == ord('3') and msg.get_srcComponent() == ord('D')) or
+            msg.get_srcSystem() == 255 or
+            msg.get_type() == 'BAD_DATA'):
+            seq = (self.last_seq+1) % 256
+            seq2 = msg.get_seq()
+            if seq != seq2 and self.last_seq != -1:
+                diff = (seq2 - seq) % 256
+                self.mav_loss += diff
+            self.last_seq = seq2
+            self.mav_count += 1
+        
         self.timestamp = msg._timestamp
         if type == 'HEARTBEAT':
             self.target_system = msg.get_srcSystem()
@@ -119,33 +168,42 @@ class mavfile(object):
             if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
                 self.flightmode = mode_string_v10(msg)
         elif type == 'PARAM_VALUE':
+            s = str(msg.param_id)
             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
-            if str(msg.param_id) == 'GND_ABS_PRESS':
-                self.ground_pressure = msg.param_value
-            if str(msg.param_id) == 'GND_TEMP':
-                self.ground_temperature = msg.param_value
         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
+        elif type == 'GPS_RAW_INT':
+            if self.messages['HOME'].fix_type < 3:
+                self.messages['HOME'] = msg
         for hook in self.message_hooks:
             hook(self, msg)
 
 
+    def packet_loss(self):
+        '''packet loss as a percentage'''
+        if self.mav_count == 0:
+            return 0
+        return (100.0*self.mav_loss)/(self.mav_count+self.mav_loss)
+
+
     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:
+            if len(s) == 0 and (len(self.mav.buf) == 0 or self.stop_on_EOF):
                 return None
             if self.logfile_raw:
                 self.logfile_raw.write(str(s))
+            if self.first_byte:
+                self.auto_mavlink_version(s)
             msg = self.mav.parse_char(s)
             if msg:
                 self.post_message(msg)
@@ -168,6 +226,10 @@ class mavfile(object):
                 continue
             return m
 
+    def mavlink10(self):
+        '''return True if using MAVLink 1.0'''
+        return self.WIRE_PROTOCOL_VERSION == "1.0"
+
     def setup_logfile(self, logfile, mode='w'):
         '''start logging to the given logfile, with timestamps'''
         self.logfile = open(logfile, mode=mode)
@@ -197,9 +259,9 @@ class mavfile(object):
 
     def param_set_send(self, parm_name, parm_value, parm_type=None):
         '''wrapper for parameter set'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             if parm_type == None:
-                parm_type = mavlink.MAV_VAR_FLOAT
+                parm_type = mavlink.MAVLINK_TYPE_FLOAT
             self.mav.param_set_send(self.target_system, self.target_component,
                                     parm_name, parm_value, parm_type)
         else:
@@ -208,35 +270,35 @@ class mavfile(object):
 
     def waypoint_request_list_send(self):
         '''wrapper for waypoint_request_list_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             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':
+        if self.mavlink10():
             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':
+        if self.mavlink10():
             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':
+        if self.mavlink10():
             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_current(self):
         '''return current waypoint'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             m = self.recv_match(type='MISSION_CURRENT', blocking=True)
         else:
             m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
@@ -244,14 +306,14 @@ class mavfile(object):
 
     def waypoint_count_send(self, seq):
         '''wrapper for waypoint_count_send'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             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)
 
     def set_mode_auto(self):
         '''enter auto mode'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.mav.command_long_send(self.target_system, self.target_component,
                                        mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0)
         else:
@@ -260,7 +322,7 @@ class mavfile(object):
 
     def set_mode_rtl(self):
         '''enter RTL mode'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.mav.command_long_send(self.target_system, self.target_component,
                                        mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
         else:
@@ -269,7 +331,7 @@ class mavfile(object):
 
     def set_mode_loiter(self):
         '''enter LOITER mode'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.mav.command_long_send(self.target_system, self.target_component,
                                        mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0)
         else:
@@ -278,7 +340,7 @@ class mavfile(object):
 
     def calibrate_imu(self):
         '''calibrate IMU'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.mav.command_long_send(self.target_system, self.target_component,
                                        mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
                                        1, 1, 1, 1, 0, 0, 0)
@@ -288,7 +350,7 @@ class mavfile(object):
 
     def calibrate_level(self):
         '''calibrate accels'''
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.mav.command_long_send(self.target_system, self.target_component,
                                        mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0,
                                        1, 1, 1, 1, 0, 0, 0)
@@ -298,7 +360,7 @@ class mavfile(object):
 
     def wait_gps_fix(self):
         self.recv_match(type='VFR_HUD', blocking=True)
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        if self.mavlink10():
             self.recv_match(type='GPS_RAW_INT', blocking=True,
                             condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0')
         else:
@@ -308,7 +370,9 @@ class mavfile(object):
     def location(self):
         '''return current location'''
         self.wait_gps_fix()
-        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+        # wait for another VFR_HUD, to ensure we have correct altitude
+        self.recv_match(type='VFR_HUD', blocking=True)
+        if self.mavlink10():
             return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
                             self.messages['GPS_RAW_INT'].lon*1.0e-7,
                             self.messages['VFR_HUD'].alt,
@@ -319,6 +383,19 @@ class mavfile(object):
                             self.messages['VFR_HUD'].alt,
                             self.messages['VFR_HUD'].heading)
 
+    def field(self, type, field, default=None):
+        '''convenient function for returning an arbitrary MAVLink
+           field with a default'''
+        if not type in self.messages:
+            return default
+        return getattr(self.messages[type], field, default)
+
+    def param(self, name, default=None):
+        '''convenient function for returning an arbitrary MAVLink
+           parameter with a default'''
+        if not name in self.params:
+            return default
+        return self.params[name]
 
 class mavserial(mavfile):
     '''a serial mavlink port'''
@@ -329,9 +406,11 @@ class mavserial(mavfile):
         self.autoreconnect = autoreconnect
         self.port = serial.Serial(self.device, self.baud, timeout=0,
                                   dsrdtr=False, rtscts=False, xonxoff=False)
-
         try:
             fd = self.port.fileno()
+            flags = fcntl.fcntl(fd, fcntl.F_GETFD)
+            flags |= fcntl.FD_CLOEXEC
+            fcntl.fcntl(fd, fcntl.F_SETFD, flags)
         except Exception:
             fd = None
         mavfile.__init__(self, fd, device, source_system=source_system)
@@ -351,7 +430,7 @@ class mavserial(mavfile):
     def write(self, buf):
         try:
             return self.port.write(buf)
-        except OSError:
+        except Exception:
             if self.autoreconnect:
                 self.reset()
             return -1
@@ -370,7 +449,7 @@ class mavserial(mavfile):
                 return
             except Exception:
                 print("Failed to reopen %s" % self.device)
-                time.sleep(1)
+                time.sleep(0.5)
         
 
 class mavudp(mavfile):
@@ -383,12 +462,16 @@ class mavudp(mavfile):
         self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         self.udp_server = input
         if input:
+            self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
             self.port.bind((a[0], int(a[1])))
         else:
             self.destination_addr = (a[0], int(a[1]))
+        flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD)
+        flags |= fcntl.FD_CLOEXEC
+        fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags)
         self.port.setblocking(0)
         self.last_address = None
-        mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
+        mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input)
 
     def close(self):
         self.port.close()
@@ -397,7 +480,7 @@ class mavudp(mavfile):
         try:
             data, self.last_address = self.port.recvfrom(300)
         except socket.error as e:
-            if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
+            if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
                 return ""
             raise
         return data
@@ -418,6 +501,8 @@ class mavudp(mavfile):
         s = self.recv()
         if len(s) == 0:
             return None
+        if self.first_byte:
+            self.auto_mavlink_version(s)
         msg = self.mav.parse_buffer(s)
         if msg is not None:
             for m in msg:
@@ -437,6 +522,9 @@ class mavtcp(mavfile):
         self.destination_addr = (a[0], int(a[1]))
         self.port.connect(self.destination_addr)
         self.port.setblocking(0)
+        flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD)
+        flags |= fcntl.FD_CLOEXEC
+        fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags)
         self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
         mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
 
@@ -485,6 +573,7 @@ class mavlogfile(mavfile):
             self._timestamp = 0
         else:
             self._timestamp = time.time()
+        self.stop_on_EOF = True
 
     def close(self):
         self.f.close()
@@ -500,7 +589,8 @@ class mavlogfile(mavfile):
     def pre_message(self):
         '''read timestamp if needed'''
         # read the timestamp
-        self.percent = (100.0 * self.f.tell()) / self.filesize
+        if self.filesize != 0:
+            self.percent = (100.0 * self.f.tell()) / self.filesize
         if self.notimestamps:
             return
         if self.planner_format:
@@ -510,12 +600,14 @@ class mavlogfile(mavfile):
             hnsec = self._two64 + float(tbuf[0:20])
             t = hnsec * 1.0e-7         # convert to seconds
             t -= 719163 * 24 * 60 * 60 # convert to 1970 base
+            self._link = 0
         else:
             tbuf = self.f.read(8)
             if len(tbuf) != 8:
                 return
             (tusec,) = struct.unpack('>Q', tbuf)
             t = tusec * 1.0e-6
+            self._link = tusec & 0x3
         self._timestamp = t
 
     def post_message(self, msg):
@@ -586,7 +678,7 @@ class periodic_event(object):
     def force(self):
         '''force immediate triggering'''
         self.last_time = 0
-
+        
     def trigger(self):
         '''return True if we should trigger now'''
         tnow = time.time()

Tools/autotest/pymavlink/mavwp.py

@@ -36,6 +36,15 @@ class MAVWPLoader(object):
         w.seq = self.count()
         self.wpoints.append(w)
 
+    def set(self, w, idx):
+        '''set a waypoint'''
+        w.seq = idx
+        if w.seq == self.count():
+            return self.add(w)
+        if self.count() <= idx:
+            raise MAVWPError('adding waypoint at idx=%u past end of list (count=%u)' % (idx, self.count()))
+        self.wpoints[idx] = w
+
     def remove(self, w):
         '''remove a waypoint'''
         self.wpoints.remove(w)