From 842a4f507b66f3a99852180e61ad4854a34ae3bf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 2 Jul 2024 12:01:20 +1000
Subject: [PATCH] AP_Scripting: added NMEA 2000 EFI driver

this has been tested on a marine engine, and correctly produces key
telemetry data

the NMEA_2000.lua module is broken out to make it easier to add other
NMEA 2000 based drivers
---
 libraries/AP_Scripting/drivers/EFI_NMEA2k.lua | 196 ++++++++++++++++++
 libraries/AP_Scripting/drivers/EFI_NMEA2k.md  |  45 ++++
 libraries/AP_Scripting/modules/NMEA_2000.lua  | 119 +++++++++++
 3 files changed, 360 insertions(+)
 create mode 100644 libraries/AP_Scripting/drivers/EFI_NMEA2k.lua
 create mode 100644 libraries/AP_Scripting/drivers/EFI_NMEA2k.md
 create mode 100644 libraries/AP_Scripting/modules/NMEA_2000.lua

diff --git a/libraries/AP_Scripting/drivers/EFI_NMEA2k.lua b/libraries/AP_Scripting/drivers/EFI_NMEA2k.lua
new file mode 100644
index 0000000000..42993852ee
--- /dev/null
+++ b/libraries/AP_Scripting/drivers/EFI_NMEA2k.lua
@@ -0,0 +1,196 @@
+--[[
+   EFI driver using NMEA 2000 marine CAN protocol
+--]]
+
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+PARAM_TABLE_KEY = 48
+PARAM_TABLE_PREFIX = "EFI_2K_"
+
+local efi_backend = nil
+local efi_state = EFI_State()
+local cylinder_state = Cylinder_Status()
+if not efi_state or not cylinder_state then
+   return
+end
+
+-- bind a parameter to a variable given
+function bind_param(name)
+    local p = Parameter(name)
+    assert(p, string.format('could not find %s parameter', name))
+    return p
+end
+
+-- add a parameter and bind it to a variable
+function bind_add_param(name, idx, default_value)
+    assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+    return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- Setup EFI Parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add EFI_2K param table')
+
+--[[
+  // @Param: EFI_2K_ENABLE
+  // @DisplayName: Enable NMEA 2000 EFI driver
+  // @Description: Enable NMEA 2000 EFI driver
+  // @Values: 0:Disabled,1:Enabled
+  // @User: Standard
+--]]
+local EFI_2K_ENABLE = bind_add_param('ENABLE',  1, 1)
+if EFI_2K_ENABLE:get() < 1 then
+   return
+end
+
+--[[
+  // @Param: EFI_2K_CANDRV
+  // @DisplayName: NMEA 2000 CAN driver
+  // @Description: NMEA 2000 CAN driver. Use 1 for first CAN scripting driver, 2 for 2nd driver
+  // @Values: 0:Disabled,1:FirstCAN,2:SecondCAN
+  // @User: Standard
+--]]
+local EFI_2K_CANDRV = bind_add_param('CANDRV',  2, 0)   -- CAN driver number
+
+--[[
+  // @Param: EFI_2K_OPTIONS
+  // @DisplayName: NMEA 2000 options
+  // @Description: NMEA 2000 driver options
+  // @Bitmask: 0:EnableLogging
+  // @User: Standard
+--]]
+EFI_2K_OPTIONS = bind_add_param("OPTIONS", 3, 0)
+
+local OPTION_LOGGING = (1<<0)
+
+--[[
+   return true if an option is enabled
+--]]
+local function option_enabled(option)
+   return (EFI_2K_OPTIONS:get() & option) ~= 0
+end
+
+-- Register for the CAN drivers
+local CAN_BUF_LEN = 25
+local can_driver = nil
+
+if EFI_2K_CANDRV:get() == 1 then
+   gcs:send_text(MAV_SEVERITY.INFO, string.format("EFI_2K: attaching to CAN1"))
+   can_driver = CAN:get_device(CAN_BUF_LEN)
+elseif EFI_2K_CANDRV:get() == 2 then
+   gcs:send_text(MAV_SEVERITY.INFO, string.format("EFI_2K: attaching to CAN2"))
+   can_driver = CAN:get_device2(CAN_BUF_LEN)
+end
+
+if not can_driver then
+   gcs:send_text(MAV_SEVERITY.ERROR, string.format("EFI_2K: invalid CAN driver"))
+   return
+end
+
+-- load NMEA_2000 module
+local NMEA_2000 = require("NMEA_2000")
+if not NMEA_2000 then
+   gcs:send_text(MAV_SEVERITY.ERROR, "EFI_2K: Unable to load NMEA_2000.lua module")
+   return
+end
+
+--[[
+   create PGN table
+--]]
+local PGN_ENGINE_PARAM_RAPID = 0x1F200
+local PGN_ENGINE_PARAM_DYNAMIC = 0x1F201
+
+local PGN_TABLE = {
+   [PGN_ENGINE_PARAM_RAPID] = 8,
+   [PGN_ENGINE_PARAM_DYNAMIC] = 26
+}
+
+NMEA_2000.set_PGN_table(PGN_TABLE)
+
+local frame_count = 0
+local state = {}
+
+local function log_frame(frame)
+   local id = frame:id()
+   logger:write("CANF",'Id,DLC,FC,B0,B1,B2,B3,B4,B5,B6,B7','IBIBBBBBBBB',
+                id,
+                frame:dlc(),
+                frame_count,
+                frame:data(0), frame:data(1), frame:data(2), frame:data(3),
+                frame:data(4), frame:data(5), frame:data(6), frame:data(7))
+   frame_count = frame_count + 1
+end
+
+--[[
+   parse the higher rate engine data, giving RPM and pressure
+--]]
+local function parse_engine_param_rapid(data)
+   state.instance, state.speed, state.boost_presssure, state.tilt, _ = string.unpack("<BHHbH", data)
+   if not state.instance then
+      return
+   end
+
+   efi_state:engine_speed_rpm(uint32_t(state.speed/4))
+   efi_state:atmospheric_pressure_kpa(state.boost_presssure*0.001)
+   efi_state:cylinder_status(cylinder_state)
+   efi_state:last_updated_ms(millis())
+
+   -- Set the EFI_State into the EFI scripting driver
+   if efi_backend then
+      efi_backend:handle_scripting(efi_state)
+   end
+end
+
+--[[
+   parse the lower rate engine data, giving other engine data
+--]]
+local function parse_engine_param_dynamic(data)
+   state.instance, state.oil_press, state.oil_temp, state.temp_K, state.alt_V, state.fuel_rate, state.engine_s, state.cool_press,
+      state.fuel_press, state.res1, state.dstat1, state.dstat2, state.eload, state.etorque = string.unpack("<BHHHhhIHHBHHbb", data)
+   if not state.instance then
+      return
+   end
+   efi_state:intake_manifold_temperature(state.temp_K*0.01)
+   efi_state:oil_pressure(state.oil_press*0.01*0.001)
+   efi_state:oil_temperature(state.oil_temp*0.1)
+   cylinder_state:cylinder_head_temperature(state.temp_K*0.01)
+   efi_state:ignition_voltage(state.alt_V*0.01)
+   efi_state:fuel_consumption_rate_cm3pm(state.fuel_rate*0.1*1000.0/3600.0)
+   efi_state:intake_manifold_pressure_kpa(state.cool_press*100*0.001)
+   efi_state:fuel_pressure(state.fuel_press)
+   efi_state:engine_load_percent(state.eload)
+end
+
+function update()
+   if EFI_2K_ENABLE:get() < 1 then
+      return update, 500
+   end
+   if not efi_backend then
+      efi_backend = efi:get_backend(0)
+   end
+   if not efi_backend then
+      return update, 500
+   end
+
+   -- read up to 25 frames per update() call
+   for _ = 1, 25 do
+      local frame = can_driver:read_frame()
+      if not frame then
+         break
+      end
+      if option_enabled(OPTION_LOGGING) then
+         log_frame(frame)
+      end
+      pgn, data = NMEA_2000.parse(frame)
+      if pgn then
+         if pgn == PGN_ENGINE_PARAM_RAPID then
+            parse_engine_param_rapid(data)
+         elseif pgn == PGN_ENGINE_PARAM_DYNAMIC then
+            parse_engine_param_dynamic(data)
+         end
+      end
+   end
+
+   return update, 2
+end
+
+return update()
diff --git a/libraries/AP_Scripting/drivers/EFI_NMEA2k.md b/libraries/AP_Scripting/drivers/EFI_NMEA2k.md
new file mode 100644
index 0000000000..82e5d735d9
--- /dev/null
+++ b/libraries/AP_Scripting/drivers/EFI_NMEA2k.md
@@ -0,0 +1,45 @@
+# NMEA 2000 EFI driver
+
+This driver implements support for marine EFI systems using NMEA 2000
+CAN messages.
+
+# Parameters
+
+The script used the following parameters:
+
+## EFI_2K_ENABLE
+
+this must be set to 1 to enable the driver
+
+## EFI_2K_OPTIONS
+
+This sets options for the driver. Currently the only option is to set
+EFI_2K_OPTIONS to 1 to enable logging of the raw CAN frames for
+debugging purposes.
+
+# 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 NMEA_2000.lua module from
+AP_Scripting/modules/ also needs to be put in the APM/SCRIPTS/MODULES directory.
+
+The following key parameters should be set:
+
+ - SCR_ENABLE should be set to 1
+ - EFI_TYPE should be set to 7
+ - EFI_2K_ENABLE should be set to 1
+ - EFI_2K_CANDRV needs to be set to the CAN driver number
+ - CAN_Pn_BITRATE needs to be set to 250000
+ - CAN_Dn_PROTOOCOL needs to be set to 10 for scripting
+
+then the flight controller should rebooted and parameters should be
+refreshed.
+
+Once loaded the EFI_2K_ENABLE parameters will appear and should be set
+according to the parameter list above.
+
+The GCS will receive EFI_STATUS MAVLink messages which includes RPM,
+cylinder head temperature, exhaust gas temperature, injection timing,
+engine load, fuel consumption rate, throttle position atmospheric
+pressure and ignition voltage.
diff --git a/libraries/AP_Scripting/modules/NMEA_2000.lua b/libraries/AP_Scripting/modules/NMEA_2000.lua
new file mode 100644
index 0000000000..3a4b63d1d9
--- /dev/null
+++ b/libraries/AP_Scripting/modules/NMEA_2000.lua
@@ -0,0 +1,119 @@
+--[[
+   NMEA 2000 parser as lua module
+   with thanks to https://canboat.github.io/canboat/canboat.html
+
+   caller must setup a PGN expected size table with set_PGN_table()
+--]]
+
+local M = {}
+
+M.PGN_table = {}
+
+-- multi-frame pending data
+M.pending = { pgn = nil, data = "", count = 0, expected_size = 0 }
+
+-- Extract the PGN (Parameter Group Number) from the message ID
+local function extract_pgn(message_id)
+   local PF = (message_id >> 16) & 0xFF
+   local RDP = (message_id >> 24) & 0x3
+   if PF < 0xF0 then
+      return (RDP << 16) | (PF << 8)
+   else
+      local PS = (message_id >> 8) & 0xFF
+      return (RDP << 16) | (PF << 8) | PS
+   end
+end
+
+--[[
+   extract data from a CAN frame as a lua binary string
+--]]
+local function extract_data(frame, max_len)
+   local ret = ""
+   local dlc = frame:dlc()
+   local len = math.min(dlc, max_len)
+   for ofs = 1, len do
+      ret = ret .. string.char(frame:data(ofs-1))
+   end
+   return ret
+end
+
+--[[
+   set table of PGNs that we are interested in along with the expected packet size
+
+   The table should be indexed by the PGN and give the expected size
+   of that PGN any frames with PGNs not in this table will be
+   discarded
+--]]
+function M.set_PGN_table(t)
+   M.PGN_table = t
+end
+
+-- Parse CAN frame and reassemble messages
+function M.parse(can_frame)
+   if not can_frame:isExtended() then
+      -- NMEA 2000 frame are always extended (29 bit address)
+      return nil
+   end
+   local message_id = can_frame:id_signed()
+
+   local pgn = extract_pgn(message_id)
+   local dlc = can_frame:dlc()
+
+   local exp_size = M.PGN_table[pgn]
+   if not exp_size then
+      -- discard unwated frame and reset pending
+      M.pending.pgn = nil
+      return nil
+   end
+
+   if exp_size <= 8 and exp_size > dlc then
+      -- discard short frame
+      M.pending.pgn = nil
+      return nil
+   end
+
+   if exp_size <= 8 then
+      -- single frame
+      local data = extract_data(can_frame, exp_size)
+      M.pending.pgn = nil
+      return pgn, data
+   end
+
+   -- multi-frame
+   local data = extract_data(can_frame, dlc)
+   local subframe = string.byte(data, 1) & 0x1F
+   if M.pending.pgn ~= pgn or subframe ~= M.pending.count then
+      -- reset
+      M.pending.pgn = nil
+      M.pending.data = ""
+      M.pending.count = 0
+
+      if subframe ~= 0 then
+         -- discard, lost first frame or out of order
+         return nil
+      end
+   end
+
+   if subframe == 0 then
+      M.pending.expected_size = string.byte(data, 2)
+      if M.pending.expected_size < exp_size then
+         M.pending.pgn = nil
+         return nil
+      end
+      M.pending.data = M.pending.data .. string.sub(data, 3, #data)
+   else
+      M.pending.data = M.pending.data .. string.sub(data, 2, #data)
+   end
+   M.pending.pgn = pgn
+   M.pending.count = M.pending.count + 1
+
+   -- do we have a complete frame
+   if #M.pending.data >= M.pending.expected_size then
+      M.pending.pgn = nil
+      return pgn, M.pending.data
+   end
+
+   return nil
+end
+
+return M