AP_Scripting: improved dual log handling

allow sync of PTHT messages between vehicle logs

libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_log.py

@@ -25,19 +25,22 @@ mlog1 = mavutil.mavlink_connection(args.log1)
 mlog2 = mavutil.mavlink_connection(args.log2)
 output = open(args.logout, mode='wb')
 
-types1 = ['ORGN','VEH','PARM','MSG','FMT','FMTU','MULT','MODE','EVT']
-types2 = ['VEH']
+types1 = ['ORGN','VEH','PARM','MSG','FMT','FMTU','MULT','MODE','EVT','PTHT']
+types2 = ['VEH','PTHT']
 
 m1 = None
 m2 = None
 
-veh_fmt = None
+veh1_formats = {}
 
-def write_VEH(m, veh_fmt):
-    buf = bytearray(m.get_msgbuf())
-    if veh_fmt is None:
+def write_message(m):
+    global veh1_formats
+    mtype = m.get_type()
+    if not mtype in veh1_formats:
         return
-    buf[2] = veh_fmt
+    veh1_fmt = veh1_formats[mtype]
+    buf = bytearray(m.get_msgbuf())
+    buf[2] = veh1_fmt
     output.write(buf)
 
 m1_count = 0
@@ -49,44 +52,41 @@ while True:
     if m2 is None:
         m2 = mlog2.recv_match(type=types2)
 
-    if m1 is not None and m1.get_type() != 'VEH':
+    if m1 is not None:
+        veh1_formats[m1.get_type()] = m1.get_msgbuf()[2]
+
+    if m1 is not None and m1.get_type() not in types2:
+        # passthrough
         output.write(m1.get_msgbuf())
         m1 = None
         continue
 
-    if m2 is not None and m2.get_type() != 'VEH':
+    if m2 is not None and m2.get_type() not in types2:
         continue
-    
-    if veh_fmt is None and m1 is not None:
-        veh_fmt = m1.get_msgbuf()[2]
 
     if m1 is None and m2 is None:
         break
 
     if m1 is None:
-        write_VEH(m2, veh_fmt)
+        write_message(m2)
         m2 = None
         continue
 
     if m2 is None:
-        write_VEH(m1, veh_fmt)
+        write_message(m1)
         m1 = None
         continue
 
-    if hasattr(m1,'TSec'):
-        # old format
-        t1 = m1.TSec + m1.TUSec*1.0e-6
-        t2 = m2.TSec + m2.TUSec*1.0e-6
-    else:
-        t1 = m1.GWk*7*24*60*60 + m1.GMS*0.001
-        t2 = m2.GWk*7*24*60*60 + m2.GMS*0.001
+    t1 = m1.GWk*7*24*60*60 + m1.GMS*0.001
+    t2 = m2.GWk*7*24*60*60 + m2.GMS*0.001
+
     if t1 <= t2:
         m1_count += 1
         if m1_count % args.decimate == 0:
-            write_VEH(m1, veh_fmt)
+            write_message(m1)
         m1 = None
     else:
         m2_count += 1
         if m2_count % args.decimate == 0:
-            write_VEH(m2, veh_fmt)
+            write_message(m2)
         m2 = None

libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua

@@ -2130,8 +2130,10 @@ function log_pose(logname, pos, quat)
                 math.deg(quat:get_euler_yaw()))
 end
 
-
-function log_position(logname, loc, quat)
+--[[
+   get GPS week and MS, coping with crossing a week boundary
+--]]
+function get_gps_times()
    local gps_last_fix_ms1 = gps:last_fix_time_ms(0)
    local gps_week = gps:time_week(0)
    local gps_week_ms = gps:time_week_ms(0)
@@ -2144,6 +2146,11 @@ function log_position(logname, loc, quat)
       gps_week_ms = gps:time_week_ms(0)
    end
    gps_week_ms = gps_week_ms + (now_ms - gps_last_fix_ms2)
+   return gps_week, gps_week_ms
+end
+
+function log_position(logname, loc, quat)
+   local gps_week, gps_week_ms = get_gps_times()
 
    logger.write(logname, 'I,GWk,GMS,Lat,Lon,Alt,R,P,Y',
                 'BHILLffff',
@@ -2359,8 +2366,15 @@ function handle_speed_adjustment()
          path_var.speed_adjustment = 0.0
       end
 
-      logger.write("PTHT",'SysID,RemT,LocT,TS,RemTS,PathT,RemPathT,Dt,ARPT,DE,SA','Bffffffffff',
-                   sysid, remote_t, local_t,
+      local gps_week, gps_week_ms = get_gps_times()
+      logger.write("PTHT",
+                   'SysID,GWk,GMS,RemT,LocT,TS,RTS,PT,RPT,Dt,ARPT,DE,SA',
+                   'BHIffffffffff',
+                   '#------------',
+                   '-------------',
+                   sysid,
+                   gps_week, gps_week_ms,
+                   remote_t, local_t,
                    loc_timestamp,
                    remote_timestamp,
                    path_var.path_t, rem_patht,