2022-09-28 02:55:05 -03:00
|
|
|
--[[
|
|
|
|
Name: EFI Scripting backend driver for SkyPower
|
|
|
|
Authors: Andrew Tridgell & Josh Henderson
|
|
|
|
|
|
|
|
The protocol has high CAN utilization due to CAN packets that are used
|
|
|
|
internal to the enigne being published externally, as well as being
|
|
|
|
limited to 500 kbit/s.
|
|
|
|
|
|
|
|
CAN_D1_PROTOCOL 10 (Scripting Driver 1)
|
|
|
|
CAN_P1_DRIVER 1 (First driver)
|
|
|
|
CAN_D1_BITRATE 500000 (500 kbit/s)
|
|
|
|
|
|
|
|
--]]
|
|
|
|
|
2024-05-03 15:22:25 -03:00
|
|
|
---@diagnostic disable: param-type-mismatch
|
|
|
|
---@diagnostic disable: need-check-nil
|
|
|
|
---@diagnostic disable: undefined-field
|
|
|
|
---@diagnostic disable: missing-parameter
|
|
|
|
|
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
-- Check Script uses a miniumum firmware version
|
2022-10-21 19:02:01 -03:00
|
|
|
local SCRIPT_AP_VERSION = 4.3
|
2022-09-28 02:55:05 -03:00
|
|
|
local SCRIPT_NAME = "EFI: Skypower CAN"
|
|
|
|
|
|
|
|
local VERSION = FWVersion:major() + (FWVersion:minor() * 0.1)
|
|
|
|
|
|
|
|
assert(VERSION >= SCRIPT_AP_VERSION, string.format('%s Requires: %s:%.1f. Found Version: %s', SCRIPT_NAME, FWVersion:type(), SCRIPT_AP_VERSION, VERSION))
|
|
|
|
|
|
|
|
|
|
|
|
local MAV_SEVERITY_ERROR = 3
|
|
|
|
|
2022-10-21 19:02:01 -03:00
|
|
|
local K_THROTTLE = 70
|
|
|
|
local K_HELIRSC = 31
|
|
|
|
|
2024-03-09 00:34:38 -04:00
|
|
|
local MODEL_SRE_180 = 0
|
2023-12-09 22:42:00 -04:00
|
|
|
local MODEL_SP_275 = 1
|
2024-03-09 00:34:38 -04:00
|
|
|
local MODEL_DEFAULT = MODEL_SRE_180
|
2023-12-09 22:42:00 -04:00
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
PARAM_TABLE_KEY = 36
|
|
|
|
PARAM_TABLE_PREFIX = "EFI_SP_"
|
|
|
|
|
|
|
|
-- bind a parameter to a variable given
|
|
|
|
function bind_param(name)
|
|
|
|
local p = Parameter()
|
|
|
|
assert(p:init(name), 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
|
|
|
|
|
|
|
|
function get_time_sec()
|
|
|
|
return millis():tofloat() * 0.001
|
|
|
|
end
|
|
|
|
|
|
|
|
-- Type conversion functions
|
|
|
|
function get_uint16(frame, ofs)
|
|
|
|
return frame:data(ofs) + (frame:data(ofs + 1) << 8)
|
|
|
|
end
|
|
|
|
|
|
|
|
function constrain(v, vmin, vmax)
|
|
|
|
if v < vmin then
|
|
|
|
v = vmin
|
|
|
|
end
|
|
|
|
if v > vmax then
|
|
|
|
v = vmax
|
|
|
|
end
|
|
|
|
return v
|
|
|
|
end
|
|
|
|
|
|
|
|
local efi_backend = nil
|
|
|
|
|
|
|
|
-- Setup EFI Parameters
|
2023-01-10 19:30:09 -04:00
|
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add EFI_SP param table')
|
2022-09-28 02:55:05 -03:00
|
|
|
|
2023-02-11 01:55:20 -04:00
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_ENABLE
|
|
|
|
// @DisplayName: Enable SkyPower EFI support
|
|
|
|
// @Description: Enable SkyPower EFI support
|
|
|
|
// @Values: 0:Disabled,1:Enabled
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_ENABLE = bind_add_param('ENABLE', 1, 0)
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_CANDRV
|
|
|
|
// @DisplayName: Set SkyPower EFI CAN driver
|
|
|
|
// @Description: Set SkyPower EFI CAN driver
|
|
|
|
// @Values: 0:None,1:1stCANDriver,2:2ndCanDriver
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_CANDRV = bind_add_param('CANDRV', 2, 1) -- CAN driver to use
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_UPDATE_HZ
|
|
|
|
// @DisplayName: SkyPower EFI update rate
|
|
|
|
// @Description: SkyPower EFI update rate
|
|
|
|
// @Range: 10 200
|
|
|
|
// @Units: Hz
|
|
|
|
// @User: Advanced
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_UPDATE_HZ = bind_add_param('UPDATE_HZ', 3, 200) -- Script update frequency in Hz
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_THR_FN
|
|
|
|
// @DisplayName: SkyPower EFI throttle function
|
|
|
|
// @Description: SkyPower EFI throttle function. This sets which SERVOn_FUNCTION to use for the target throttle. This should be 70 for fixed wing aircraft and 31 for helicopter rotor speed control
|
|
|
|
// @Values: 0:Disabled,70:FixedWing,31:HeliRSC
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_THR_FN = bind_add_param('THR_FN', 4, 0) -- servo function for throttle
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_THR_RATE
|
|
|
|
// @DisplayName: SkyPower EFI throttle rate
|
|
|
|
// @Description: SkyPower EFI throttle rate. This sets rate at which throttle updates are sent to the engine
|
|
|
|
// @Range: 10 100
|
|
|
|
// @Units: Hz
|
|
|
|
// @User: Advanced
|
|
|
|
--]]
|
2023-01-10 19:30:09 -04:00
|
|
|
local EFI_SP_THR_RATE = bind_add_param('THR_RATE', 5, 50) -- throttle update rate
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_START_FN
|
|
|
|
// @DisplayName: SkyPower EFI start function
|
|
|
|
// @Description: SkyPower EFI start function. This is the RCn_OPTION value to use to find the R/C channel used for controlling engine start
|
|
|
|
// @Values: 0:Disabled,300:300,301:301,302:302,303:303,304:304,305:305,306:306,307:307
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_START_FN = bind_add_param('START_FN', 6, 0) -- start control function (RC option)
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_GEN_FN
|
|
|
|
// @DisplayName: SkyPower EFI generator control function
|
|
|
|
// @Description: SkyPower EFI generator control function. This is the RCn_OPTION value to use to find the R/C channel used for controlling generator start/stop
|
|
|
|
// @Values: 0:Disabled,300:300,301:301,302:302,303:303,304:304,305:305,306:306,307:307
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2022-10-21 19:02:01 -03:00
|
|
|
local EFI_SP_GEN_FN = bind_add_param('GEN_FN', 7, 0) -- generator control function (RC option)
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_MIN_RPM
|
|
|
|
// @DisplayName: SkyPower EFI minimum RPM
|
|
|
|
// @Description: SkyPower EFI minimum RPM. This is the RPM below which the engine is considered to be stopped
|
|
|
|
// @Range: 1 1000
|
|
|
|
// @User: Advanced
|
|
|
|
--]]
|
2023-01-10 19:30:09 -04:00
|
|
|
local EFI_SP_MIN_RPM = bind_add_param('MIN_RPM', 8, 100) -- min RPM, for engine restart
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_TLM_RT
|
|
|
|
// @DisplayName: SkyPower EFI telemetry rate
|
|
|
|
// @Description: SkyPower EFI telemetry rate. This is the rate at which extra telemetry values are sent to the GCS
|
|
|
|
// @Range: 1 10
|
|
|
|
// @Units: Hz
|
|
|
|
// @User: Advanced
|
|
|
|
--]]
|
2023-01-10 19:30:09 -04:00
|
|
|
local EFI_SP_TLM_RT = bind_add_param('TLM_RT', 9, 0) -- rate for extra telemetry values
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_LOG_RT
|
|
|
|
// @DisplayName: SkyPower EFI log rate
|
|
|
|
// @Description: SkyPower EFI log rate. This is the rate at which extra logging of the SkyPower EFI is performed
|
|
|
|
// @Range: 1 50
|
|
|
|
// @Units: Hz
|
|
|
|
// @User: Advanced
|
|
|
|
--]]
|
2023-01-10 19:30:09 -04:00
|
|
|
local EFI_SP_LOG_RT = bind_add_param('LOG_RT', 10, 10) -- rate for logging
|
2023-02-11 01:55:20 -04:00
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_ST_DISARM
|
|
|
|
// @DisplayName: SkyPower EFI allow start disarmed
|
|
|
|
// @Description: SkyPower EFI allow start disarmed. This controls if starting the engine while disarmed is allowed
|
|
|
|
// @Values: 0:Disabled,1:Enabled
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
2023-01-10 19:30:09 -04:00
|
|
|
local EFI_SP_ST_DISARM = bind_add_param('ST_DISARM', 11, 0) -- allow start when disarmed
|
2022-09-28 02:55:05 -03:00
|
|
|
|
2023-12-09 22:42:00 -04:00
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_MODEL
|
|
|
|
// @DisplayName: SkyPower EFI ECU model
|
|
|
|
// @Description: SkyPower EFI ECU model
|
2024-03-09 00:34:38 -04:00
|
|
|
// @Values: 0:SRE_180,1:SP_275
|
2023-12-09 22:42:00 -04:00
|
|
|
// @User: Standard
|
|
|
|
--]]
|
|
|
|
local EFI_SP_MODEL = bind_add_param('MODEL', 12, MODEL_DEFAULT)
|
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_GEN_CTRL
|
|
|
|
// @DisplayName: SkyPower EFI enable generator control
|
|
|
|
// @Description: SkyPower EFI enable generator control
|
|
|
|
// @Values: 0:Disabled,1:Enabled
|
|
|
|
// @User: Standard
|
|
|
|
--]]
|
|
|
|
local EFI_SP_GEN_CTRL = bind_add_param('GEN_CRTL', 13, 1)
|
|
|
|
|
|
|
|
--[[
|
|
|
|
// @Param: EFI_SP_RST_TIME
|
|
|
|
// @DisplayName: SkyPower EFI restart time
|
|
|
|
// @Description: SkyPower EFI restart time. If engine should be running and it has stopped for this amount of time then auto-restart. To disable this feature set this value to zero.
|
|
|
|
// @Range: 0 10
|
|
|
|
// @User: Standard
|
|
|
|
// @Units: s
|
|
|
|
--]]
|
|
|
|
local EFI_SP_RST_TIME = bind_add_param('RST_TIME', 14, 2)
|
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
if EFI_SP_ENABLE:get() == 0 then
|
2022-10-24 19:27:15 -03:00
|
|
|
gcs:send_text(0, string.format("EFISP: disabled"))
|
2022-09-28 02:55:05 -03:00
|
|
|
return
|
|
|
|
end
|
|
|
|
|
2022-10-21 19:02:01 -03:00
|
|
|
-- Register for the CAN drivers
|
|
|
|
local driver1
|
|
|
|
|
|
|
|
local CAN_BUF_LEN = 25
|
|
|
|
if EFI_SP_CANDRV:get() == 1 then
|
|
|
|
driver1 = CAN.get_device(CAN_BUF_LEN)
|
|
|
|
elseif EFI_SP_CANDRV:get() == 2 then
|
|
|
|
driver1 = CAN.get_device2(CAN_BUF_LEN)
|
|
|
|
end
|
|
|
|
|
|
|
|
if not driver1 then
|
|
|
|
gcs:send_text(0, string.format("EFISP: Failed to load driver"))
|
|
|
|
return
|
|
|
|
end
|
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
local now_s = get_time_sec()
|
|
|
|
|
2022-10-24 19:27:15 -03:00
|
|
|
function C_TO_KELVIN(temp)
|
|
|
|
return temp + 273.15
|
|
|
|
end
|
|
|
|
|
2023-01-10 19:30:09 -04:00
|
|
|
--[[
|
|
|
|
we allow the engine to run if either armed or the EFI_SP_ST_DISARM parameter is 1
|
|
|
|
--]]
|
|
|
|
function allow_run_engine()
|
|
|
|
return arming:is_armed() or EFI_SP_ST_DISARM:get() == 1
|
|
|
|
end
|
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
--[[
|
|
|
|
EFI Engine Object
|
|
|
|
--]]
|
2023-12-09 22:42:00 -04:00
|
|
|
local function engine_control(_driver)
|
2022-09-28 02:55:05 -03:00
|
|
|
local self = {}
|
|
|
|
|
|
|
|
-- Build up the EFI_State that is passed into the EFI Scripting backend
|
|
|
|
local efi_state = EFI_State()
|
|
|
|
local cylinder_state = Cylinder_Status()
|
|
|
|
|
|
|
|
-- private fields as locals
|
|
|
|
local rpm = 0
|
|
|
|
local air_pressure = 0
|
|
|
|
local inj_time = 0
|
|
|
|
local target_load = 0
|
|
|
|
local current_load = 0
|
|
|
|
local throttle_angle = 0
|
|
|
|
local ignition_angle = 0
|
|
|
|
local supply_voltage = 0
|
2024-03-09 00:34:38 -04:00
|
|
|
local supply_voltage2 = 0
|
2022-09-28 02:55:05 -03:00
|
|
|
local fuel_consumption_lph = 0
|
|
|
|
local fuel_total_l = 0
|
|
|
|
local last_fuel_s = 0
|
|
|
|
local driver = _driver
|
|
|
|
local last_rpm_t = get_time_sec()
|
|
|
|
local last_state_update_t = get_time_sec()
|
2022-10-21 19:02:01 -03:00
|
|
|
local last_thr_update = get_time_sec()
|
2023-01-10 19:30:09 -04:00
|
|
|
local last_telem_update = get_time_sec()
|
|
|
|
local last_log_t = get_time_sec()
|
|
|
|
local last_stop_message_t = get_time_sec()
|
2022-10-21 19:02:01 -03:00
|
|
|
local engine_started = false
|
|
|
|
local generator_started = false
|
|
|
|
local engine_start_t = 0.0
|
2023-01-10 19:30:09 -04:00
|
|
|
local last_throttle = 0.0
|
2023-12-09 22:42:00 -04:00
|
|
|
local sensor_error_flags = 0
|
|
|
|
local thermal_limit_flags = 0
|
|
|
|
local starter_rpm = 0
|
2022-10-21 19:02:01 -03:00
|
|
|
|
|
|
|
-- frames for sending commands
|
|
|
|
local FRM_500 = uint32_t(0x500)
|
|
|
|
local FRM_505 = uint32_t(0x505)
|
|
|
|
local FRM_506 = uint32_t(0x506)
|
2022-09-28 02:55:05 -03:00
|
|
|
|
|
|
|
-- Generator Data Structure
|
|
|
|
local gen = {}
|
|
|
|
gen.amps = 0.0
|
|
|
|
gen.rpm = 0.0
|
|
|
|
gen.batt_current = 0.0
|
|
|
|
|
|
|
|
-- Temperature Data Structure
|
|
|
|
local temps = {}
|
2022-10-24 19:27:15 -03:00
|
|
|
temps.egt = 0.0 -- Exhaust Gas Temperature
|
|
|
|
temps.cht = 0.0 -- Cylinder Head Temperature
|
|
|
|
temps.imt = 0.0 -- intake manifold temperature
|
2023-01-10 19:30:09 -04:00
|
|
|
temps.oilt = 0.0 -- oil temperature
|
2023-12-09 22:42:00 -04:00
|
|
|
temps.cht2 = 0.0
|
|
|
|
temps.egt2 = 0.0
|
|
|
|
temps.imt2 = 0.0
|
|
|
|
temps.oil2 = 0.0
|
2022-09-28 02:55:05 -03:00
|
|
|
|
|
|
|
-- read telemetry packets
|
|
|
|
function self.update_telemetry()
|
|
|
|
local max_packets = 25
|
|
|
|
local count = 0
|
|
|
|
while count < max_packets do
|
|
|
|
frame = driver:read_frame()
|
|
|
|
count = count + 1
|
|
|
|
if not frame then
|
|
|
|
break
|
|
|
|
end
|
|
|
|
|
2024-03-09 00:34:38 -04:00
|
|
|
--[[
|
|
|
|
note that some SkyPower setups use extended and some
|
|
|
|
11-bit CAN frames
|
|
|
|
--]]
|
|
|
|
self.handle_EFI_packet(frame)
|
2022-09-28 02:55:05 -03:00
|
|
|
end
|
|
|
|
if last_rpm_t > last_state_update_t then
|
|
|
|
-- update state if we have an updated RPM
|
|
|
|
last_state_update_t = last_rpm_t
|
|
|
|
self.set_EFI_State()
|
|
|
|
end
|
|
|
|
end
|
|
|
|
|
|
|
|
-- handle an EFI packet
|
2023-12-09 22:42:00 -04:00
|
|
|
function self.handle_EFI_packet(frame)
|
2022-09-28 02:55:05 -03:00
|
|
|
local id = frame:id_signed()
|
2023-12-09 22:42:00 -04:00
|
|
|
if EFI_SP_MODEL:get() == MODEL_SP_275 then
|
|
|
|
-- updated telemetry for SP-275 ECU
|
|
|
|
if id == 0x100 then
|
|
|
|
rpm = get_uint16(frame, 0)
|
|
|
|
ignition_angle = get_uint16(frame, 2) * 0.1
|
|
|
|
throttle_angle = get_uint16(frame, 4) * 0.1
|
|
|
|
last_rpm_t = get_time_sec()
|
|
|
|
elseif id == 0x101 then
|
|
|
|
air_pressure = get_uint16(frame, 4)
|
|
|
|
elseif id == 0x102 then
|
|
|
|
inj_time = get_uint16(frame, 4)
|
|
|
|
-- inj_ang = get_uint16(frame, 2) * 0.1
|
|
|
|
elseif id == 0x104 then
|
|
|
|
supply_voltage = get_uint16(frame, 0) * 0.1
|
|
|
|
elseif id == 0x105 then
|
|
|
|
temps.cht = get_uint16(frame, 0) * 0.1
|
|
|
|
temps.imt = get_uint16(frame, 2) * 0.1
|
|
|
|
temps.egt = get_uint16(frame, 4) * 0.1
|
|
|
|
temps.oilt = get_uint16(frame, 6) * 0.1
|
|
|
|
elseif id == 0x107 then
|
|
|
|
sensor_error_flags = get_uint16(frame, 0)
|
|
|
|
thermal_limit_flags = get_uint16(frame, 2)
|
|
|
|
elseif id == 0x107 then
|
|
|
|
target_load = get_uint16(frame, 6) * 0.1
|
|
|
|
elseif id == 0x10C then
|
|
|
|
temps.cht2 = get_uint16(frame, 4) * 0.1
|
|
|
|
temps.egt2 = get_uint16(frame, 6) * 0.1
|
|
|
|
elseif id == 0x10D then
|
|
|
|
current_load = get_uint16(frame, 2) * 0.1
|
|
|
|
elseif id == 0x113 then
|
|
|
|
gen.amps = get_uint16(frame, 2)
|
2024-03-09 00:34:38 -04:00
|
|
|
elseif id == 0x114 then
|
|
|
|
supply_voltage2 = get_uint16(frame, 4) * 0.01
|
2023-12-09 22:42:00 -04:00
|
|
|
elseif id == 0x2E0 then
|
|
|
|
starter_rpm = get_uint16(frame, 4)
|
|
|
|
elseif id == 0x10B then
|
|
|
|
fuel_consumption_lph = get_uint16(frame,6)*0.001
|
|
|
|
if last_fuel_s > 0 then
|
|
|
|
local dt = now_s - last_fuel_s
|
|
|
|
local fuel_lps = fuel_consumption_lph / 3600.0
|
|
|
|
fuel_total_l = fuel_total_l + fuel_lps * dt
|
|
|
|
end
|
|
|
|
last_fuel_s = now_s
|
|
|
|
end
|
|
|
|
else
|
2024-03-09 00:34:38 -04:00
|
|
|
assert(EFI_SP_MODEL:get() == MODEL_SRE_180, string.format('%s Invalid EFI_SP_MODEL', SCRIPT_NAME))
|
|
|
|
-- original SkyPower driver, SRE_180
|
2023-12-09 22:42:00 -04:00
|
|
|
if id == 0x100 then
|
|
|
|
rpm = get_uint16(frame, 0)
|
|
|
|
ignition_angle = get_uint16(frame, 2) * 0.1
|
|
|
|
throttle_angle = get_uint16(frame, 4) * 0.1
|
|
|
|
last_rpm_t = get_time_sec()
|
|
|
|
elseif id == 0x101 then
|
|
|
|
current_load = get_uint16(frame, 0) * 0.1
|
|
|
|
target_load = get_uint16(frame, 2) * 0.1
|
|
|
|
inj_time = get_uint16(frame, 4)
|
|
|
|
-- inj_ang = get_uint16(frame, 6) * 0.1
|
|
|
|
elseif id == 0x104 then
|
|
|
|
supply_voltage = get_uint16(frame, 0) * 0.1
|
|
|
|
ecu_temp = get_uint16(frame, 2) * 0.1
|
|
|
|
air_pressure = get_uint16(frame, 4)
|
|
|
|
fuel_consumption_lph = get_uint16(frame,6)*0.001
|
|
|
|
if last_fuel_s > 0 then
|
|
|
|
local dt = now_s - last_fuel_s
|
|
|
|
local fuel_lps = fuel_consumption_lph / 3600.0
|
|
|
|
fuel_total_l = fuel_total_l + fuel_lps * dt
|
|
|
|
end
|
|
|
|
last_fuel_s = now_s
|
|
|
|
elseif id == 0x105 then
|
|
|
|
temps.cht = get_uint16(frame, 0) * 0.1
|
|
|
|
temps.imt = get_uint16(frame, 2) * 0.1
|
|
|
|
temps.egt = get_uint16(frame, 4) * 0.1
|
|
|
|
temps.oilt = get_uint16(frame, 6) * 0.1
|
2024-03-09 00:34:38 -04:00
|
|
|
elseif id == 0x178 then
|
|
|
|
supply_voltage2 = get_uint16(frame, 4) * 0.01
|
2022-09-28 02:55:05 -03:00
|
|
|
end
|
|
|
|
end
|
|
|
|
end
|
|
|
|
|
|
|
|
-- Build and set the EFI_State that is passed into the EFI Scripting backend
|
|
|
|
function self.set_EFI_State()
|
|
|
|
-- Cylinder_Status
|
2022-10-24 19:27:15 -03:00
|
|
|
cylinder_state:cylinder_head_temperature(C_TO_KELVIN(temps.cht))
|
|
|
|
cylinder_state:exhaust_gas_temperature(C_TO_KELVIN(temps.egt))
|
2022-09-28 02:55:05 -03:00
|
|
|
cylinder_state:ignition_timing_deg(ignition_angle)
|
|
|
|
cylinder_state:injection_time_ms(inj_time*0.001)
|
|
|
|
|
|
|
|
efi_state:engine_speed_rpm(uint32_t(rpm))
|
2023-01-10 19:30:09 -04:00
|
|
|
efi_state:engine_load_percent(math.floor(current_load))
|
2022-09-28 02:55:05 -03:00
|
|
|
|
|
|
|
efi_state:fuel_consumption_rate_cm3pm(fuel_consumption_lph * 1000.0 / 60.0)
|
|
|
|
efi_state:estimated_consumed_fuel_volume_cm3(fuel_total_l * 1000.0)
|
|
|
|
efi_state:throttle_position_percent(math.floor(throttle_angle*100.0/90.0+0.5))
|
|
|
|
efi_state:atmospheric_pressure_kpa(air_pressure*0.1)
|
|
|
|
efi_state:ignition_voltage(supply_voltage)
|
2022-10-24 19:27:15 -03:00
|
|
|
efi_state:intake_manifold_temperature(C_TO_KELVIN(temps.imt))
|
2023-01-10 19:30:09 -04:00
|
|
|
efi_state:throttle_out(last_throttle * 100)
|
2022-09-28 02:55:05 -03:00
|
|
|
|
|
|
|
-- copy cylinder_state to efi_state
|
|
|
|
efi_state:cylinder_status(cylinder_state)
|
|
|
|
|
2023-12-09 22:42:00 -04:00
|
|
|
local last_efi_state_time = millis()
|
2022-09-28 02:55:05 -03:00
|
|
|
efi_state:last_updated_ms(last_efi_state_time)
|
|
|
|
|
|
|
|
|
|
|
|
-- Set the EFI_State into the EFI scripting driver
|
|
|
|
efi_backend:handle_scripting(efi_state)
|
|
|
|
end
|
|
|
|
|
2022-10-21 19:02:01 -03:00
|
|
|
--- send throttle command, thr is 0 to 1
|
|
|
|
function self.send_throttle(thr)
|
2023-01-10 19:30:09 -04:00
|
|
|
last_throttle = thr
|
2022-10-21 19:02:01 -03:00
|
|
|
local msg = CANFrame()
|
|
|
|
msg:id(FRM_500)
|
|
|
|
msg:data(0,1)
|
|
|
|
msg:data(1,0)
|
|
|
|
thr = math.floor(thr*1000)
|
|
|
|
msg:data(2,thr&0xFF)
|
|
|
|
msg:data(3,thr>>8)
|
|
|
|
msg:dlc(8)
|
|
|
|
driver:write_frame(msg, 10000)
|
|
|
|
end
|
|
|
|
|
|
|
|
-- send an engine start command
|
|
|
|
function self.send_engine_start()
|
2023-12-09 22:42:00 -04:00
|
|
|
if EFI_SP_MODEL:get() == MODEL_SP_275 then
|
|
|
|
-- the SP-275 needs a stop before a start will work
|
|
|
|
self.send_engine_stop()
|
|
|
|
end
|
2022-10-21 19:02:01 -03:00
|
|
|
local msg = CANFrame()
|
|
|
|
msg:id(FRM_505)
|
|
|
|
msg:data(0,10)
|
|
|
|
msg:dlc(8)
|
|
|
|
driver:write_frame(msg, 10000)
|
|
|
|
end
|
|
|
|
|
|
|
|
-- send an engine stop command
|
|
|
|
function self.send_engine_stop()
|
|
|
|
local msg = CANFrame()
|
|
|
|
msg:id(FRM_505)
|
|
|
|
msg:data(7,10)
|
|
|
|
msg:dlc(8)
|
|
|
|
driver:write_frame(msg, 10000)
|
2023-01-10 19:30:09 -04:00
|
|
|
local now = get_time_sec()
|
|
|
|
if now - last_stop_message_t > 0.5 then
|
|
|
|
last_stop_message_t = now
|
|
|
|
gcs:send_text(0, string.format("EFISP: stopping engine"))
|
|
|
|
end
|
2022-10-21 19:02:01 -03:00
|
|
|
end
|
|
|
|
|
|
|
|
-- start generator
|
|
|
|
function self.send_generator_start()
|
|
|
|
local msg = CANFrame()
|
|
|
|
msg:id(FRM_506)
|
|
|
|
msg:data(2,10)
|
|
|
|
msg:dlc(8)
|
|
|
|
driver:write_frame(msg, 10000)
|
|
|
|
end
|
|
|
|
|
|
|
|
-- stop generator
|
|
|
|
function self.send_generator_stop()
|
|
|
|
local msg = CANFrame()
|
|
|
|
msg:id(FRM_506)
|
|
|
|
msg:data(2,0)
|
|
|
|
msg:dlc(8)
|
|
|
|
driver:write_frame(msg, 10000)
|
|
|
|
end
|
|
|
|
|
|
|
|
-- update starter control
|
|
|
|
function self.update_starter()
|
|
|
|
local start_fn = EFI_SP_START_FN:get()
|
|
|
|
if start_fn == 0 then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
local start_state = rc:get_aux_cached(start_fn)
|
2023-01-10 19:30:09 -04:00
|
|
|
if start_state == nil then
|
|
|
|
start_state = 0
|
|
|
|
end
|
|
|
|
local should_be_running = false
|
2022-10-21 19:02:01 -03:00
|
|
|
if start_state == 0 and engine_started then
|
|
|
|
engine_started = false
|
|
|
|
engine_start_t = 0
|
|
|
|
self.send_engine_stop()
|
|
|
|
end
|
2023-01-10 19:30:09 -04:00
|
|
|
if start_state == 2 and not engine_started and allow_run_engine() then
|
2022-10-21 19:02:01 -03:00
|
|
|
engine_started = true
|
|
|
|
gcs:send_text(0, string.format("EFISP: starting engine"))
|
|
|
|
engine_start_t = get_time_sec()
|
|
|
|
self.send_engine_start()
|
2023-01-10 19:30:09 -04:00
|
|
|
should_be_running = true
|
|
|
|
end
|
|
|
|
if start_state > 0 and engine_started and allow_run_engine() then
|
|
|
|
should_be_running = true
|
2022-10-21 19:02:01 -03:00
|
|
|
end
|
|
|
|
local min_rpm = EFI_SP_MIN_RPM:get()
|
2023-01-10 19:30:09 -04:00
|
|
|
if min_rpm > 0 and engine_started and rpm < min_rpm and allow_run_engine() then
|
2022-10-21 19:02:01 -03:00
|
|
|
local now = get_time_sec()
|
|
|
|
local dt = now - engine_start_t
|
2023-12-09 22:42:00 -04:00
|
|
|
if EFI_SP_RST_TIME:get() > 0 and dt > EFI_SP_RST_TIME:get() then
|
2022-10-21 19:02:01 -03:00
|
|
|
gcs:send_text(0, string.format("EFISP: re-starting engine"))
|
|
|
|
self.send_engine_start()
|
2023-01-10 19:30:09 -04:00
|
|
|
engine_start_t = get_time_sec()
|
2022-10-21 19:02:01 -03:00
|
|
|
end
|
|
|
|
end
|
2023-01-10 19:30:09 -04:00
|
|
|
--[[
|
|
|
|
cope with lost engine stop packets
|
|
|
|
--]]
|
|
|
|
if rpm > min_rpm and not should_be_running then
|
|
|
|
engine_started = false
|
|
|
|
engine_start_t = 0
|
|
|
|
self.send_engine_stop()
|
|
|
|
end
|
2022-10-21 19:02:01 -03:00
|
|
|
end
|
|
|
|
|
|
|
|
-- update generator control
|
|
|
|
function self.update_generator()
|
2023-12-09 22:42:00 -04:00
|
|
|
if EFI_SP_GEN_CTRL:get() == 0 then
|
|
|
|
return
|
|
|
|
end
|
2022-10-21 19:02:01 -03:00
|
|
|
local gen_state = rc:get_aux_cached(EFI_SP_GEN_FN:get())
|
|
|
|
if gen_state == 0 and generator_started then
|
|
|
|
generator_started = false
|
|
|
|
gcs:send_text(0, string.format("EFISP: stopping generator"))
|
|
|
|
self.send_generator_stop()
|
|
|
|
end
|
|
|
|
if gen_state == 2 and not generator_started then
|
|
|
|
generator_started = true
|
|
|
|
gcs:send_text(0, string.format("EFISP: starting generator"))
|
|
|
|
self.send_generator_start()
|
|
|
|
end
|
|
|
|
end
|
|
|
|
|
|
|
|
-- update throttle output
|
|
|
|
function self.update_throttle()
|
|
|
|
local thr_func = EFI_SP_THR_FN:get()
|
|
|
|
local thr_rate = EFI_SP_THR_RATE:get()
|
|
|
|
if thr_func == 0 or thr_rate == 0 then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
local now = get_time_sec()
|
|
|
|
if now - last_thr_update < 1.0 / thr_rate then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
last_thr_update = now
|
|
|
|
local thr = 0.0
|
|
|
|
local scaled = SRV_Channels:get_output_scaled(thr_func)
|
|
|
|
if thr_func == K_THROTTLE then
|
|
|
|
thr = scaled * 0.01
|
|
|
|
elseif thr_func == K_HELIRSC then
|
|
|
|
thr = scaled * 0.001
|
|
|
|
end
|
|
|
|
self.send_throttle(thr)
|
|
|
|
end
|
2023-01-10 19:30:09 -04:00
|
|
|
|
|
|
|
-- update telemetry output for extra telemetry values
|
|
|
|
function self.update_telem_out()
|
|
|
|
local rate = EFI_SP_TLM_RT:get()
|
|
|
|
if rate <= 0 then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
local now = get_time_sec()
|
|
|
|
if now - last_telem_update < 1.0 / rate then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
last_telem_update = now
|
|
|
|
gcs:send_named_float('EFI_OILTMP', temps.oilt)
|
|
|
|
gcs:send_named_float('EFI_TRLOAD', target_load)
|
|
|
|
gcs:send_named_float('EFI_VOLTS', supply_voltage)
|
2024-03-09 00:34:38 -04:00
|
|
|
gcs:send_named_float('EFI_VOLTS2', supply_voltage2)
|
2023-12-09 22:42:00 -04:00
|
|
|
gcs:send_named_float('EFI_GEN_AMPS', gen.amps)
|
|
|
|
gcs:send_named_float('EFI_CHT2', temps.cht2)
|
|
|
|
gcs:send_named_float('EFI_STARTRPM', starter_rpm)
|
2023-01-10 19:30:09 -04:00
|
|
|
end
|
|
|
|
|
|
|
|
-- update custom logging
|
|
|
|
function self.update_logging()
|
|
|
|
local rate = EFI_SP_LOG_RT:get()
|
|
|
|
if rate <= 0 then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
local now = get_time_sec()
|
|
|
|
if now - last_log_t < 1.0 / rate then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
last_log_t = now
|
2023-12-09 22:42:00 -04:00
|
|
|
logger.write('EFSP','Thr,CLoad,TLoad,OilT,RPM,gRPM,gAmp,gCur,SErr,TLim,STRPM', 'ffffffffHHH',
|
2023-01-10 19:30:09 -04:00
|
|
|
last_throttle, current_load, target_load, temps.oilt, rpm,
|
2023-12-09 22:42:00 -04:00
|
|
|
gen.rpm, gen.amps, gen.batt_current,
|
|
|
|
sensor_error_flags, thermal_limit_flags,
|
|
|
|
starter_rpm)
|
2023-01-10 19:30:09 -04:00
|
|
|
end
|
2022-10-21 19:02:01 -03:00
|
|
|
|
2022-09-28 02:55:05 -03:00
|
|
|
-- return the instance
|
|
|
|
return self
|
2023-12-09 22:42:00 -04:00
|
|
|
end -- end function engine_control
|
2022-09-28 02:55:05 -03:00
|
|
|
|
2023-12-09 22:42:00 -04:00
|
|
|
local engine1 = engine_control(driver1)
|
2022-09-28 02:55:05 -03:00
|
|
|
|
|
|
|
function update()
|
|
|
|
now_s = get_time_sec()
|
|
|
|
|
|
|
|
if not efi_backend then
|
|
|
|
efi_backend = efi:get_backend(0)
|
|
|
|
if not efi_backend then
|
|
|
|
return
|
|
|
|
end
|
|
|
|
end
|
|
|
|
|
|
|
|
-- Parse Driver Messages
|
|
|
|
engine1.update_telemetry()
|
2022-10-21 19:02:01 -03:00
|
|
|
engine1.update_starter()
|
|
|
|
engine1.update_generator()
|
|
|
|
engine1.update_throttle()
|
2023-01-10 19:30:09 -04:00
|
|
|
engine1.update_telem_out()
|
|
|
|
engine1.update_logging()
|
2022-09-28 02:55:05 -03:00
|
|
|
end
|
|
|
|
|
|
|
|
gcs:send_text(0, SCRIPT_NAME .. string.format(" loaded"))
|
|
|
|
|
|
|
|
-- wrapper around update(). This calls update() and if update faults
|
|
|
|
-- then an error is displayed, but the script is not stopped
|
|
|
|
function protected_wrapper()
|
|
|
|
local success, err = pcall(update)
|
|
|
|
if not success then
|
|
|
|
gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " .. err)
|
|
|
|
-- when we fault we run the update function again after 1s, slowing it
|
|
|
|
-- down a bit so we don't flood the console with errors
|
|
|
|
return protected_wrapper, 1000
|
|
|
|
end
|
|
|
|
return protected_wrapper, 1000 / EFI_SP_UPDATE_HZ:get()
|
|
|
|
end
|
|
|
|
|
|
|
|
-- start running update loop
|
|
|
|
return protected_wrapper()
|