Tools: support CANFD log playback in CAN_playback.py

Tools/scripts/CAN/CAN_playback.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 '''
-playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus
+ playback a set of CAN frames
+ capture frames either using libraries/AP_Scripting/examples/CAN_logger.lua
+ or the CAN_Pn_OPTIONS bit to enable CAN logging
 '''
 
 import dronecan
@@ -9,6 +11,7 @@ import sys
 import threading
 from pymavlink import mavutil
 from dronecan.driver.common import CANFrame
+import struct
 
 
 # get command line arguments
@@ -16,6 +19,7 @@ from argparse import ArgumentParser
 parser = ArgumentParser(description='CAN playback')
 parser.add_argument("logfile", default=None, type=str, help="logfile")
 parser.add_argument("canport", default=None, type=str, help="CAN port")
+parser.add_argument("--bus", default=0, type=int, help="CAN bus")
 
 args = parser.parse_args()
 
@@ -28,8 +32,27 @@ mlog = mavutil.mavlink_connection(args.logfile)
 tstart = time.time()
 first_tstamp = None
 
+def dlc_to_datalength(dlc):
+    # Data Length Code      9  10  11  12  13  14  15
+    # Number of data bytes 12  16  20  24  32  48  64
+    if (dlc <= 8):
+        return dlc
+    elif (dlc == 9):
+        return 12
+    elif (dlc == 10):
+        return 16
+    elif (dlc == 11):
+        return 20
+    elif (dlc == 12):
+        return 24
+    elif (dlc == 13):
+        return 32
+    elif (dlc == 14):
+        return 48
+    return 64
+
 while True:
-    m = mlog.recv_match(type='CANF')
+    m = mlog.recv_match(type=['CANF','CAFD'])
 
     if m is None:
         print("Rewinding")
@@ -38,15 +61,25 @@ while True:
         first_tstamp = None
         continue
 
+    if getattr(m,'bus',0) != args.bus:
+        continue
+
     if first_tstamp is None:
         first_tstamp = m.TimeUS
     dt = time.time() - tstart
     dt2 = (m.TimeUS - first_tstamp)*1.0e-6
     if dt2 > dt:
         time.sleep(dt2 - dt)
-    data = [m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7]
+
-    data = data[:m.DLC]
+    canfd = m.get_type() == 'CAFD'
+    if canfd:
+        data = struct.pack("<QQQQQQQQ", m.D0, m.D1, m.D2, m.D3, m.D4, m.D5, m.D6, m.D7)
+        data = data[:dlc_to_datalength(m.DLC)]
+    else:
+        data = struct.pack("<BBBBBBBB", m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7)
+        data = data[:m.DLC]
+
     fid = m.Id
     is_extended = (fid & (1<<31)) != 0
-    driver.send(fid, data, extended=is_extended, canfd=False)
+    driver.send(fid, data, extended=is_extended, canfd=canfd)
     print(m)