mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
Andrew Tridgell
parent
9090420db0
commit
842a4f507b
|
|
@ -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()
|
||||
|
|
@ -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.
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue