Merge branch 'ArduPilot:master' into master

Author: duguguang

MAVProxy/modules/lib/grapher.py

@@ -381,7 +381,7 @@ def plotit(self, x, y, fields, colors=[], title=None, interactive=True):
         if self.grid:
             pylab.grid()
 
-        if self.show_flightmode:
+        if self.show_flightmode != 0:
             alpha = 0.3
             xlim = self.ax1.get_xlim()
             for i in range(len(self.flightmode_list)):
@@ -409,14 +409,14 @@ def plotit(self, x, y, fields, colors=[], title=None, interactive=True):
         else:
             self.fig.canvas.set_window_title(title)
 
-        if self.show_flightmode:
+        if self.show_flightmode != 0:
             mode_patches = []
             for mode in self.modes_plotted.keys():
                 (color, alpha) = self.modes_plotted[mode]
                 mode_patches.append(matplotlib.patches.Patch(color=color,
                                                              label=mode, alpha=alpha*1.5))
             labels = [patch.get_label() for patch in mode_patches]
-            if ax1_labels != []:
+            if ax1_labels != [] and self.show_flightmode != 2:
                 patches_legend = matplotlib.pyplot.legend(mode_patches, labels, loc=self.legend_flightmode)
                 self.fig.gca().add_artist(patches_legend)
             else:

MAVProxy/modules/lib/mission_item_protocol.py

@@ -59,6 +59,7 @@ def __init__(self, mpstate, name, description, **args):
 
     def gui_menu_items(self):
         return [
+            MPMenuItem('FTP', 'FTP', '# %s ftp' % self.command_name()),
             MPMenuItem('Clear', 'Clear', '# %s clear' % self.command_name()),
             MPMenuItem('List', 'List', '# %s list' % self.command_name()),
             MPMenuItem(
@@ -863,6 +864,8 @@ def commands(self):
             print("%s module not available; use old compat modules" % str(self.itemtype()))
             return
         return {
+            "ftp": self.wp_ftp_download,
+            "ftpload": self.wp_ftp_upload,
             "clear": self.cmd_clear,
             "list": self.cmd_list,
             "load": (self.cmd_load, ["(FILENAME)"]),
@@ -962,7 +965,7 @@ def request_list_send(self):
             mission_type=self.mav_mission_type())
 
     def wp_ftp_download(self, args):
-        '''Download wpts from vehicle with ftp'''
+        '''Download items from vehicle with ftp'''
         ftp = self.mpstate.module('ftp')
         if ftp is None:
             print("Need ftp module")
@@ -971,7 +974,7 @@ def wp_ftp_download(self, args):
         ftp.cmd_get([self.mission_ftp_name()], callback=self.ftp_callback, callback_progress=self.ftp_callback_progress)
 
     def ftp_callback_progress(self, fh, total_size):
-        '''progress callback from ftp fetch of mission'''
+        '''progress callback from ftp fetch of mission items'''
         if self.ftp_count is None and total_size >= 10:
             ofs = fh.tell()
             fh.seek(0)
@@ -987,18 +990,18 @@ def ftp_callback_progress(self, fh, total_size):
             self.mpstate.console.set_status('Mission', 'Mission %u/%u' % (done, self.ftp_count))
 
     def ftp_callback(self, fh):
-        '''callback from ftp fetch of mission'''
+        '''callback from ftp fetch of mission items'''
         if fh is None:
             print("mission: failed ftp download")
             return
         magic = 0x763d
         data = fh.read()
         magic2, dtype, options, start, num_items = struct.unpack("<HHHHH", data[0:10])
         if magic != magic2:
-            print("mission: bad magic 0x%x expected 0x%x" % (magic2, magic))
+            print("%s: bad magic 0x%x expected 0x%x" % (self.itemtype(), magic2, magic))
             return
-        if dtype != mavutil.mavlink.MAV_MISSION_TYPE_MISSION:
-            print("mission: bad data type %u" % dtype)
+        if dtype != self.mav_mission_type():
+            print("%s: bad data type %u" % (self.itemtype(), dtype))
             return
 
         self.wploader.clear()
@@ -1051,11 +1054,11 @@ def wp_ftp_upload(self, args):
         except Exception as msg:
             print("Unable to load %s - %s" % (filename, msg))
             return
-        print("Loaded %u waypoints from %s" % (self.wploader.count(), filename))
-        print("Sending mission with ftp")
+        print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename))
+        print("Sending %s with ftp" % self.itemstype())
 
         fh = SIO()
-        fh.write(struct.pack("<HHHHH", 0x763d, mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 0, 0, self.wploader.count()))
+        fh.write(struct.pack("<HHHHH", 0x763d, self.mav_mission_type(), 0, 0, self.wploader.count()))
         mavmsg = mavutil.mavlink.MAVLink_mission_item_int_message
         for i in range(self.wploader.count()):
             w = self.wploader.wp(i)
@@ -1074,18 +1077,18 @@ def wp_ftp_upload(self, args):
                     fh=fh, callback=self.ftp_upload_callback, progress_callback=self.ftp_upload_progress)
 
     def ftp_upload_progress(self, proportion):
-        '''callback from ftp put of mission'''
+        '''callback from ftp put of items'''
         if proportion is None:
             self.mpstate.console.set_status('Mission', 'Mission ERR')
         else:
             count = self.wploader.count()
             self.mpstate.console.set_status('Mission', 'Mission %u/%u' % (int(proportion*count), count))
 
     def ftp_upload_callback(self, dlen):
-        '''callback from ftp put of mission'''
+        '''callback from ftp put of items'''
         if dlen is None:
-            print("Failed to send waypoints")
+            print("Failed to send %s" % self.itemstype())
         else:
             mavmsg = mavutil.mavlink.MAVLink_mission_item_int_message
             item_size = mavmsg.unpacker.size
-            print("Sent mission of length %u in %.2fs" % ((dlen - 10) // item_size, time.time() - self.upload_start))
+            print("Sent %s of length %u in %.2fs" % (self.itemtype(), (dlen - 10) // item_size, time.time() - self.upload_start))

MAVProxy/modules/lib/mp_module.py

@@ -205,9 +205,11 @@ def link_label(link):
             label = str(link.linknum+1)
         return label
 
-    def is_primary_vehicle(self, msg):
+    def message_is_from_primary_vehicle(self, msg):
         '''see if a msg is from our primary vehicle'''
         sysid = msg.get_srcSystem()
-        if self.target_system == 0 or self.target_system == sysid:
+        compid = msg.get_srcComponent()
+        if ((self.target_system == 0 or self.target_system == sysid) and
+           (self.target_component == 0 or self.target_component == compid)):
             return True
         return False

MAVProxy/modules/lib/msgstats.py

@@ -42,31 +42,52 @@ def child_task(self):
 
 def show_stats(mlog):
     '''show stats on a file'''
-    if not hasattr(mlog, 'formats'):
-        print("Must be DF log")
-        return
     sizes = {}
     total_size = 0
     names = mlog.name_to_id.keys()
     pairs = []
+    maxnamelen = 4
 
     for name in names:
         sizes[name] = 0
 
     for name in names:
         mid = mlog.name_to_id[name]
-        count = mlog.counts[mid]
-        mlen = mlog.formats[mid].len
-        size = count * mlen
-        total_size += size
-        sizes[name] += size
-        pairs.append((name, count*mlen))
+        # DFReader_text logs use the name directly as the index to the dictionaries
+        if hasattr(mlog, 'formats') and mid not in mlog.formats:
+            mid = name
+        # In DFReader_text logs count is a dictionary, which is not set for
+        # messages that are never seen in the log.
+        if isinstance(mlog.counts,dict) and mid not in mlog.counts:
+            count = 0
+        else:
+            count = mlog.counts[mid]
+        # mavmmaplog class (tlogs) does not contain formats attribute, so instead of
+        # counting size in bytes, we count the number of messages
+        if hasattr(mlog, 'formats'):
+            mlen = mlog.formats[mid].len
+            size = count * mlen
+            total_size += size
+            sizes[name] += size
+            pairs.append((name, count*mlen))
+        else:
+            total_size += count
+            pairs.append((name, count))
+            if count>0 and len(name)>maxnamelen:
+                maxnamelen = len(name)
+
+    # mavmmaplog class (tlogs) does not contain formats attribute, so instead of
+    # counting size in bytes, we count the number of messages
+    if not hasattr(mlog, 'formats'):
+        print("Total number of messages: %u" % total_size)
+    else:
+        print("Total size: %u" % total_size)
 
+    # Print out the percentage for each message, from lowest to highest
     pairs = sorted(pairs, key = lambda p : p[1])
-    print("Total size: %u" % total_size)
     for (name,size) in pairs:
         if size > 0:
-            print("%-4s %.2f%%" % (name, 100.0 * size / total_size))
+            print("%-*s %.2f%%" % (maxnamelen, name, 100.0 * size / total_size))
 
     print("")
     category_total = 0

MAVProxy/modules/lib/multiproc_util.py

@@ -269,7 +269,7 @@ def __init__(self, *args, **kwargs):
         '''
         Parameters
         ----------
-        mlog : DFReader
+        mlog : DFReader / mavmmaplog
             A dataflash or telemetry log
         '''
         super(MPDataLogChildTask, self).__init__(*args, **kwargs)
@@ -282,22 +282,31 @@ def wrap(self):
         '''Apply custom pickle wrappers to non-pickleable attributes'''
 
         # wrap filehandle and mmap in mlog for pickling
-        filehandle = self._mlog.filehandle
+        if hasattr(self._mlog,'filehandle'):
+            filehandle = self._mlog.filehandle
+        elif hasattr(self._mlog,'f'):
+            filehandle = self._mlog.f
         data_map = self._mlog.data_map
         data_len = self._mlog.data_len
-        self._mlog.filehandle = WrapFileHandle(filehandle)
+        if hasattr(self._mlog,'filehandle'):
+            self._mlog.filehandle = WrapFileHandle(filehandle)
+        elif hasattr(self._mlog,'f'):
+            self._mlog.f = WrapFileHandle(filehandle)
         self._mlog.data_map = WrapMMap(data_map, filehandle, data_len)
 
     # @override
     def unwrap(self):
         '''Unwrap custom pickle wrappers of non-pickleable attributes'''
 
         # restore the state of mlog
-        self._mlog.filehandle = self._mlog.filehandle.unwrap()
+        if hasattr(self._mlog,'filehandle'):
+            self._mlog.filehandle = self._mlog.filehandle.unwrap()
+        elif hasattr(self._mlog,'f'):
+            self._mlog.f = self._mlog.f.unwrap()
         self._mlog.data_map = self._mlog.data_map.unwrap()
 
     @property
     def mlog(self):
-        '''The dataflash log (DFReader)'''
+        '''The dataflash or telemetry log (DFReader / mavmmaplog)'''
 
         return self._mlog

MAVProxy/modules/lib/ntrip.py

@@ -14,8 +14,9 @@
 from MAVProxy.modules.lib import rtcm3
 import ssl
 from optparse import OptionParser
+from pynmeagps import NMEAMessage, GET
 
-version = 0.1
+version = 0.2
 useragent = "NTRIP MAVProxy/%.1f" % version
 
 
@@ -70,26 +71,8 @@ def __init__(self,
             self.ssl = True
 
     def setPosition(self, lat, lon):
-        self.flagN = "N"
-        self.flagE = "E"
-        if lon > 180:
-            lon = (lon-360)*-1
-            self.flagE = "W"
-        elif lon < 0 and lon >= -180:
-            lon = lon*-1
-            self.flagE = "W"
-        elif lon < -180:
-            lon = lon+360
-            self.flagE = "E"
-        else:
-            self.lon = lon
-        if lat < 0:
-            lat = lat*-1
-            self.flagN = "S"
-        self.lonDeg = int(lon)
-        self.latDeg = int(lat)
-        self.lonMin = (lon-self.lonDeg)*60
-        self.latMin = (lat-self.latDeg)*60
+        self.lon = lon
+        self.lat = lat
 
     def getMountPointString(self):
         userstr = self.user
@@ -105,19 +88,28 @@ def getMountPointString(self):
         mountPointString += "\r\n"
         return mountPointString
 
-    def getGGAString(self):
-        now = datetime.datetime.utcnow()
-        ggaString = "GPGGA,%02d%02d%04.2f,%02d%011.8f,%1s,%03d%011.8f,%1s,1,05,0.19,+00400,M,%5.3f,M,," % (
-                     now.hour, now.minute, now.second, self.latDeg, self.latMin, self.flagN,
-                     self.lonDeg, self.lonMin, self.flagE, self.height)
-        checksum = self.calculateCheckSum(ggaString)
-        return "$%s*%s\r\n" % (ggaString, checksum)
+    def getGGAByteString(self):
+        gga_msg = NMEAMessage(
+            "GP",
+            "GGA",
+            GET,  # msgmode is expected by this lib
+            lat=self.lat,
+            NS="S" if self.lat < 0 else "N",
+            lon=self.lon,
+            EW="W" if self.lon < 0 else "E",
+            quality=1,
+            numSV=15,
+            HDOP=0,
+            alt=self.height,
+            altUnit="M",
+            sep=0,
+            sepUnit="M",
+            diffAge="",
+            diffStation=0,
+        )
 
-    def calculateCheckSum(self, stringToCheck):
-        xsum_calc = 0
-        for char in stringToCheck:
-            xsum_calc = xsum_calc ^ ord(char)
-        return "%02X" % xsum_calc
+        raw_gga: bytes = gga_msg.serialize()
+        return raw_gga
 
     def get_ID(self):
         '''get ID of last packet'''
@@ -248,9 +240,8 @@ def readLoop(self):
             print("got: ", len(data))
 
     def send_gga(self):
-        gga = self.getGGAString()
-        if sys.version_info.major >= 3:
-            gga = bytearray(gga, "ascii")
+        gga = self.getGGAByteString()
+
         try:
             self.socket.sendall(gga)
             self.dt_last_gga_sent = time.time()

MAVProxy/modules/mavproxy_console.py

@@ -336,7 +336,7 @@ def mavlink_packet(self, msg):
         if type == 'SYS_STATUS':
             self.check_critical_error(msg)
 
-        if not self.is_primary_vehicle(msg):
+        if not self.message_is_from_primary_vehicle(msg):
             # don't process msgs from other than primary vehicle, other than
             # updating vehicle list
             return
@@ -540,13 +540,6 @@ def mavlink_packet(self, msg):
             self.console.set_status('PWR', status, fg=fg)
             self.console.set_status('Srv', 'Srv %.2f' % (msg.Vservo*0.001), fg='green')
         elif type in ['HEARTBEAT', 'HIGH_LATENCY2']:
-            if msg.get_srcComponent() in [mavutil.mavlink.MAV_COMP_ID_ADSB,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_1,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_2,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_3]:
-                # ignore these
-                return
-
             fmode = master.flightmode
             if self.settings.vehicle_name:
                 fmode = self.settings.vehicle_name + ':' + fmode