AP_Scripting: added logging and checksum fix to INF_Inject driver

Tools/scripts/serial_playback.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+'''
+playback a capture from ardupilot with timing
+the file format is a sequence of:
+
+  HEADER
+  DATA
+
+HEADER is:
+   uint32_t magic == 0x7fe53b04
+   uint32_t time_ms
+   uint32_t length
+'''
+
+import socket
+import time
+import struct
+from argparse import ArgumentParser
+
+parser = ArgumentParser(description="playback a capture file with ArduPilot timing headers")
+
+parser.add_argument("infile", default=None, help="input file")
+parser.add_argument("dest", default=None, help="TCP destination in ip:port format")
+parser.add_argument("--loop", action='store_true', help="loop to start of file at EOF")
+args = parser.parse_args()
+
+def open_socket(dest):
+    a = dest.split(':')
+    ip = a[0]
+    port = int(a[1])
+    tcp = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    tcp.connect((ip, port))
+    return tcp
+
+f = open(args.infile,'rb')
+tcpsock = open_socket(args.dest)
+
+last_ms = None
+
+while True:
+    hdr_raw = f.read(12)
+    if len(hdr_raw) != 12:
+        if args.loop:
+            f.seek(0)
+            continue
+        print("EOF")
+        break
+    magic, t_ms, n = struct.unpack("<III", hdr_raw)
+    if magic != 0x7fe53b04:
+        print("Bad magic")
+        break
+    if last_ms is not None:
+        dt = t_ms - last_ms
+        if dt > 0:
+            time.sleep(dt*0.001)
+    last_ms = t_ms
+    data = f.read(n)
+    if len(data) != n:
+        if args.loop:
+            f.seek(0)
+            continue
+        print("short data, EOF")
+        break
+    tcpsock.send(data)
+    print("Wrote %u bytes t=%.3f" % (len(data), t_ms*0.001))

libraries/AP_Scripting/drivers/INF_Inject.lua

@@ -34,11 +34,30 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add p
 --]]
 EFI_INF_ENABLE = bind_add_param("ENABLE", 1, 1)
 
+--[[
+  // @Param: EFI_INF_OPTIONS
+  // @DisplayName: EFI INF-Inject options
+  // @Description: EFI INF driver options
+  // @Bitmask: 0:EnableLogging
+  // @User: Standard
+--]]
+EFI_INF_OPTIONS = bind_add_param("OPTIONS", 2, 0)
+
+local OPTION_LOGGING = (1<<0)
+
+--[[
+   return true if an option is enabled
+--]]
+local function option_enabled(option)
+   return (EFI_INF_OPTIONS:get() & option) ~= 0
+end
+
 if EFI_INF_ENABLE:get() ~= 1 then
    return
 end
 
 local EFI_FUEL_DENS = bind_param("EFI_FUEL_DENS")
+local SCR_VM_I_COUNT = bind_param("SCR_VM_I_COUNT")
 
 local uart = serial:find_serial(0) -- first scripting serial
 if not uart then
@@ -53,12 +72,38 @@ if not efi_backend then
    return
 end
 
+--[[
+   we need a bit more time in this driver
+--]]
+if SCR_VM_I_COUNT:get() < 50000 then
+   gcs:send_text(MAV_SEVERITY.INFO, "EFI_INF: raising SCR_VM_I_COUNT to 50000")
+   SCR_VM_I_COUNT:set_and_save(50000)
+end
+
 local state = {}
 state.last_read_us = uint32_t(0)
 state.chk0 = 0
 state.chk1 = 0
 state.total_fuel_g = 0.0
 
+local file_handle = nil
+
+--[[
+   log a set of bytes
+--]]
+local function log_bytes(s)
+   if not file_handle then
+      file_handle = io.open("INF_Inject.log", "w")
+   end
+   if file_handle then
+      local magic = 0x7fe53b04
+      local now_ms = millis():toint()
+      local hdr = string.pack("<III", magic, now_ms, string.len(s))
+      file_handle:write(hdr)
+      file_handle:write(s)
+   end
+end
+
 local function read_bytes(n)
    local ret = ""
    for _ = 1, n do
@@ -67,6 +112,9 @@ local function read_bytes(n)
       state.chk1 = state.chk1 ~ state.chk0
       ret = ret .. string.char(b)
    end
+   if option_enabled(OPTION_LOGGING) then
+      log_bytes(ret)
+   end
    return ret
 end
 
@@ -94,12 +142,12 @@ local function check_input()
    end
 
    local tus = micros()
-   state.chk0 = 0
-   state.chk1 = 0
 
    -- sync on header start
    local header_ok = false
    while n_bytes >= packet_size and not header_ok do
+      state.chk0 = 0
+      state.chk1 = 0
       local header0 = string.unpack("<B", read_bytes(1))
       n_bytes = n_bytes - 1
       if header0 == 0xB5 then
@@ -161,10 +209,8 @@ local function check_input()
    local chk0 = state.chk0
    local chk1 = state.chk1
    state.check0, state.check1 = string.unpack("<BB", read_bytes(2))
-   --[[
-      we accept 0 or the right value for check1 as some devices always send 0 for chk1
-   --]]
-   local checksum_ok = chk0 == state.check0 and (0x00 == state.check1 or chk1 == state.check1)
+
+   local checksum_ok = chk0 == state.check0 and chk1 == state.check1
    if not checksum_ok then
       gcs:send_text(MAV_SEVERITY.INFO, string.format("chksum wrong (0x%02x,0x%02x) (0x%02x,0x%02x)", chk0, chk1, state.check0, state.check1))
       return false

libraries/AP_Scripting/drivers/INF_Inject.md

@@ -13,13 +13,20 @@ The script used the following parameters:
 
 this must be set to 1 to enable the driver
 
+## EFI_INF_OPTIONS
+
+This sets options for the driver. Currently the only option is to set
+EFI_INF_OPTIONS to 1 to enable logging of the raw serial bytes to a
+file called INF_Inject.log
+
 # Operation
 
 This driver should be loaded by placing the lua script in the
 APM/SCRIPTS directory on the microSD card, which can be done either
 directly or via MAVFTP. The following key parameters should be set:
 
  - SCR_ENABLE should be set to 1
+ - SCR_VM_I_COUNT should be set to at least 50000
  - EFI_TYPE should be set to 7
  - EFI_INF_ENABLE should be set to 1
  - SERIALn_PROTOCOL should be set to 28 for the connected EFI serial