mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-04 21:14:59 -06:00
139 lines
3.4 KiB
Lua
139 lines
3.4 KiB
Lua
--[[
|
|
playback CANF frames from a log captured with CAN_Pn_OPTIONS=1
|
|
|
|
Note! The log should have been stripped of everything but CANF messages, like this:
|
|
|
|
mavlogdump.py -q -o CANF.bin inlog.BIN --type CANF
|
|
|
|
For detailed instructions see CAN_playback.md
|
|
|
|
This script replays ArrayCommand for playing back CAN servo commands. Edit below for
|
|
playing back other frame types
|
|
--]]
|
|
|
|
local LOGNAME = "CANF.bin"
|
|
|
|
local can_drivers = { CAN:get_device(25), CAN:get_device2(25) }
|
|
|
|
if not can_drivers[1] and not can_drivers[2] then
|
|
gcs:send_text(0,"No scripting CAN interfaces found")
|
|
return
|
|
end
|
|
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
|
|
local tstart = millis()
|
|
|
|
local logf = io.open(LOGNAME, "r")
|
|
assert(logf ~= nil, "failed to open CANF.bin")
|
|
|
|
local next_frame = nil
|
|
local first_frame_ms = nil
|
|
local frame_ms = nil
|
|
|
|
local CANF_len = 25
|
|
|
|
local ArrayCommand = 0x0803F20A
|
|
|
|
local driver = nil
|
|
|
|
local msg = CANFrame()
|
|
msg:id(uint32_t(ArrayCommand) | uint32_t(0x80000000))
|
|
|
|
local frame_buf = nil
|
|
local frame_buf_ofs = 0
|
|
|
|
--[[
|
|
get a frame from a frame buffer
|
|
--]]
|
|
local function get_frame()
|
|
local buf_count = 100
|
|
if frame_buf == nil then
|
|
-- read 100 at a time
|
|
frame_buf = logf:read(CANF_len*buf_count)
|
|
frame_buf_ofs = 0
|
|
end
|
|
local frame = string.sub(frame_buf, frame_buf_ofs+1, frame_buf_ofs+CANF_len)
|
|
if #frame ~= CANF_len then
|
|
frame_buf = logf:read(CANF_len*buf_count)
|
|
frame_buf_ofs = 0
|
|
if frame_buf == nil then
|
|
return nil
|
|
end
|
|
frame = string.sub(frame_buf, frame_buf_ofs+1, frame_buf_ofs+CANF_len)
|
|
if #frame ~= CANF_len then
|
|
return nil
|
|
end
|
|
end
|
|
frame_buf_ofs = frame_buf_ofs + CANF_len
|
|
return frame
|
|
end
|
|
|
|
--[[
|
|
check for a new frame
|
|
--]]
|
|
local function check_frame()
|
|
local frame = get_frame()
|
|
if not frame or #frame ~= CANF_len then
|
|
gcs:send_text(MAV_SEVERITY.INFO, "CAN_playback: rewind")
|
|
logf:seek("set", 0)
|
|
first_frame_ms = nil
|
|
tstart = millis()
|
|
return
|
|
end
|
|
local h0, h1, _, t1, t2, bus, id, DLC = string.unpack('<BBBIIBIB', string.sub(frame, 1, 18))
|
|
local tus = uint64_t(t2, t1)
|
|
local tms = tus / 1000
|
|
assert(h0 == 0xA3 and h1 == 0x95, "CAN_playback: bad frame")
|
|
local mask = 0x0FFFFFFF
|
|
if (id & mask) == ArrayCommand then
|
|
if first_frame_ms == nil then
|
|
first_frame_ms = tms
|
|
end
|
|
local data = string.sub(frame, 18)
|
|
driver = can_drivers[bus+1]
|
|
if driver ~= nil then
|
|
for i=1, 8 do
|
|
if i <= DLC then
|
|
msg:data(i-1, data:byte(i))
|
|
else
|
|
msg:data(i-1, 0)
|
|
end
|
|
end
|
|
msg:dlc(DLC)
|
|
frame_ms = tms
|
|
next_frame = frame
|
|
end
|
|
end
|
|
end
|
|
|
|
--[[
|
|
read frames from the log and write to the bus when they match
|
|
--]]
|
|
function read_log()
|
|
for _ = 1, 100 do
|
|
if not next_frame then
|
|
check_frame()
|
|
end
|
|
if next_frame then
|
|
local now = millis()
|
|
if (frame_ms - first_frame_ms):toint() > (now - tstart):toint() then
|
|
-- not due yet
|
|
return
|
|
end
|
|
if driver and driver:write_frame(msg, 10000) then
|
|
next_frame = nil
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
local function update()
|
|
read_log()
|
|
return update, 1
|
|
end
|
|
|
|
gcs:send_text(MAV_SEVERITY.INFO, "CAN_playback: starting")
|
|
|
|
return update(),1000
|