mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-11 20:59:22 +08:00
Andrew Tridgell
eec4032d98
DataFlash field names are case-sensitive, so the lowercase lookup always fell back to bus zero. Read the CANF and CAFD Bus field using its declared capitalization.
108 lines
2.9 KiB
Python
Executable File
108 lines
2.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
# flake8: noqa
|
|
|
|
'''
|
|
playback a set of CAN frames
|
|
capture frames by setting bit 0 of CAN_Pn_OPTIONS to enable CAN logging
|
|
'''
|
|
|
|
import dronecan
|
|
import time
|
|
import sys
|
|
import threading
|
|
from pymavlink import mavutil
|
|
from dronecan.driver.common import CANFrame
|
|
import struct
|
|
|
|
|
|
# get command line arguments
|
|
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")
|
|
parser.add_argument("--nodes", default=None, type=str, help="comma separated list of source node IDs to send")
|
|
parser.add_argument("--messageids", default=None, type=str, help="comma separated list of DroneCAN message IDs to send")
|
|
|
|
args = parser.parse_args()
|
|
|
|
node_filter = None
|
|
if args.nodes is not None:
|
|
node_filter = set(int(n) for n in args.nodes.split(','))
|
|
|
|
msgid_filter = None
|
|
if args.messageids is not None:
|
|
msgid_filter = set(int(n, 0) for n in args.messageids.split(','))
|
|
|
|
print("Connecting to %s" % args.canport)
|
|
driver = dronecan.driver.make_driver(args.canport)
|
|
|
|
print("Opening %s" % args.logfile)
|
|
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','CAFD'])
|
|
|
|
if m is None:
|
|
print("Rewinding")
|
|
mlog.rewind()
|
|
tstart = time.time()
|
|
first_tstamp = None
|
|
continue
|
|
|
|
if getattr(m,'Bus',0) != args.bus:
|
|
continue
|
|
|
|
if node_filter is not None and (m.Id & 0x7F) not in node_filter:
|
|
continue
|
|
|
|
if msgid_filter is not None:
|
|
is_message = (m.Id & 0x7F) != 0 and (m.Id & (1 << 7)) == 0
|
|
msgId = ((m.Id >> 8) & 0xFFFF)
|
|
if not is_message or msgId not in msgid_filter:
|
|
print("skip 0x%04x" % msgId, is_message)
|
|
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)
|
|
|
|
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=canfd)
|
|
print(m)
|