pymavlink: added --link option to mavextract.py

pymavlink/tools/mavextract.py

@@ -13,11 +13,25 @@
 parser.add_argument("--robust", action='store_true', help="Enable robust parsing (skip over bad data)")
 parser.add_argument("--condition", default=None, help="select packets by condition")
 parser.add_argument("--mode", default='auto', help="mode to extract")
+parser.add_argument("--link", default=None, type=int, help="only extract specific comms link")
 parser.add_argument("logs", metavar="LOG", nargs="+")
 args = parser.parse_args()
 
 from pymavlink import mavutil
 
+def older_message(m, lastm):
+    '''return true if m is older than lastm by timestamp'''
+    atts = {'time_boot_ms' : 1.0e-3,
+            'time_unix_usec' : 1.0e-6,
+            'time_usec' : 1.0e-6}
+    for a in atts.keys():
+        if hasattr(m, a):
+            mul = atts[a]
+            t1 = m.getattr(a) * mul
+            t2 = lastm.getattr(a) * mul
+            if t2 >= t1 and t2 - t1 < 60:
+                return True
+    return False
 
 def process(filename):
     '''process one logfile'''
@@ -40,10 +54,28 @@ def process(filename):
 
     file_header = ''
 
+    messages = []
+
+    # we allow a list of modes that map to one mode number. This allows for --mode=AUTO,RTL and consider the RTL as part of AUTO
+    modes = args.mode.upper().split(',')
+    flightmode = None
+
     while True:
         m = mlog.recv_match()
         if m is None:
             break
+        if args.link is not None and m._link != args.link:
+            continue
+
+        mtype = m.get_type()
+        if mtype in messages:
+            if older_message(m, messages[mtype]):
+                continue
+
+        # we don't use mlog.flightmode as that can be wrong if we are extracting a single link
+        if mtype == 'HEARTBEAT' and m.get_srcComponent() != mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.type != mavutil.mavlink.MAV_TYPE_GCS:
+            flightmode = mavutil.mode_string_v10(m).upper()
+
         if (isbin or islog) and m.get_type() in ["FMT", "PARM", "CMD"]:
             file_header += m.get_msgbuf()
         if (isbin or islog) and m.get_type() == 'MSG' and m.Message.startswith("Ardu"):
@@ -55,9 +87,9 @@ def process(filename):
         if not mavutil.evaluate_condition(args.condition, mlog.messages):
             continue
 
-        if mlog.flightmode.upper() == args.mode.upper():
+        if flightmode in modes:
             if output is None:
-                path = os.path.join(dirname, "%s%u.%s" % (args.mode, count, extension))
+                path = os.path.join(dirname, "%s%u.%s" % (modes[0], count, extension))
                 count += 1
                 print("Creating %s" % path)
                 output = open(path, mode='wb')