aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Scripting: added throttle and generator control for EFI_SkyPower driver

Browse Source 
and added documentation for the driver
This commit is contained in:
Andrew Tridgell 2022-10-22 09:02:01 +11:00 committed by Randy Mackay
parent 85e5ddca5e
commit 2538490c60
2 changed files with 249 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
libraries/AP_Scripting/drivers/EFI_SkyPower.lua
View File

@ -13,7 +13,7 @@ CAN_D1_BITRATE 500000 (500 kbit/s)
--]]
-- Check Script uses a miniumum firmware version
local SCRIPT_AP_VERSION = 4.4
local SCRIPT_AP_VERSION = 4.3
local SCRIPT_NAME = "EFI: Skypower CAN"
local VERSION = FWVersion:major() + (FWVersion:minor() * 0.1)
@ -23,6 +23,9 @@ assert(VERSION >= SCRIPT_AP_VERSION, string.format('%s Requires: %s:%.1f. Found
local MAV_SEVERITY_ERROR = 3
local K_THROTTLE = 70
local K_HELIRSC = 31
PARAM_TABLE_KEY = 36
PARAM_TABLE_PREFIX = "EFI_SP_"
@ -62,26 +65,40 @@ end
local GKWH_TO_LBS_HP_HR = 0.0016439868
local LITRES_TO_LBS = 1.6095 -- 6.1 lbs of fuel per gallon -> 1.6095
-- Register for the CAN drivers
local driver1 = CAN.get_device(25)
if not driver1 then
gcs:send_text(0, string.format("EFI CAN Telemetry: Failed to load drivers"))
return
end
local efi_backend = nil
-- Setup EFI Parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 5), 'could not add EFI_SP param table')
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add EFI_SP param table')
local EFI_SP_ENABLE = bind_add_param('ENABLE', 1, 0)
local EFI_SP_UPDATE_HZ = bind_add_param('UPDATE_HZ', 2, 200) -- Script update frequency in Hz
local EFI_SP_ENABLE = bind_add_param('ENABLE', 1, 0)
local EFI_SP_CANDRV = bind_add_param('CANDRV', 2, 1) -- CAN driver to use
local EFI_SP_UPDATE_HZ = bind_add_param('UPDATE_HZ', 3, 200) -- Script update frequency in Hz
local EFI_SP_THR_FN = bind_add_param('THR_FN', 4, 0) -- servo function for throttle
local EFI_SP_THR_RATE = bind_add_param('THR_RATE', 5, 0) -- throttle update rate
local EFI_SP_START_FN = bind_add_param('START_FN', 6, 0) -- start control function (RC option)
local EFI_SP_GEN_FN = bind_add_param('GEN_FN', 7, 0) -- generator control function (RC option)
local EFI_SP_MIN_RPM = bind_add_param('MIN_RPM', 8, 0) -- min RPM, for engine restart
if EFI_SP_ENABLE:get() == 0 then
return
end
-- 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
local now_s = get_time_sec()
--[[
@ -114,6 +131,15 @@ local function engine_control(_driver, _idx)
local idx = _idx
local last_rpm_t = get_time_sec()
local last_state_update_t = get_time_sec()
local last_thr_update = get_time_sec()
local engine_started = false
local generator_started = false
local engine_start_t = 0.0
-- frames for sending commands
local FRM_500 = uint32_t(0x500)
local FRM_505 = uint32_t(0x505)
local FRM_506 = uint32_t(0x506)
-- Generator Data Structure
local gen = {}
@ -221,6 +247,123 @@ local function engine_control(_driver, _idx)
efi_backend:handle_scripting(efi_state)
end
--- send throttle command, thr is 0 to 1
function self.send_throttle(thr)
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()
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)
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)
if start_state == 0 and engine_started then
engine_started = false
gcs:send_text(0, string.format("EFISP: stopping engine"))
engine_start_t = 0
self.send_engine_stop()
end
if start_state == 2 and not engine_started then
engine_started = true
gcs:send_text(0, string.format("EFISP: starting engine"))
engine_start_t = get_time_sec()
self.send_engine_start()
end
local min_rpm = EFI_SP_MIN_RPM:get()
if min_rpm > 0 and engine_started and rpm < min_rpm then
local now = get_time_sec()
local dt = now - engine_start_t
if dt > 2.0 then
gcs:send_text(0, string.format("EFISP: re-starting engine"))
engine_start_t = get_time_sec()
self.send_engine_start()
end
end
end
-- update generator control
function self.update_generator()
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
-- return the instance
return self
end -- end function engine_control(_driver, _idx)
@ -241,6 +384,9 @@ function update()
-- Parse Driver Messages
engine1.update_telemetry()
engine1.update_starter()
engine1.update_generator()
engine1.update_throttle()
end
gcs:send_text(0, SCRIPT_NAME .. string.format(" loaded"))

91
libraries/AP_Scripting/drivers/EFI_SkyPower.md Normal file
View File

@ -0,0 +1,91 @@
# EFI SkyPower Driver
This driver implements support for the SkyPower range of EFI engine
control units. It supports monitoring and control of SkyPower engines.
# Parameters
The script used the following parameters:
## EFI_SP_ENABLE
this must be set to 1 to enable the driver
## EFI_SP_CANDRV
This sets the CAN scripting driver number to attach to. This is
normally set to 1 to use a CAN driver with CAN_Dx_PROTOCOL=10. To use
the 2nd scripting CAN driver set this to 2 and set CAN_Dx_PROTOCOL=12.
## EFI_SP_UPDATE_HZ
This sets the update rate of the script in Hz (how often it checks for
new data from the ECU). A value of 200 is reasonable.
## EFI_SP_THR_FN
This sets the SERVOn_FUNCTION number to monitor for throttle
command. For fixed wing forward throttle this should be set to 70. For
heli RSC control this should be set to 31. If set to zero then no
throttle control will be done by the driver.
## EFI_SPI_THR_RATE
This is the throttle output rate in Hz. A value of zero will disable
throttle control. A typical rate would be 50Hz.
## EFI_SP_START_FN
This is the RC option to use to monitor start control. This should be
set to one of the scripting RC options (from 300 to 307). Then an
RCn_OPTION should be set to the same value. When this switch goes high
the engine start function will be sent to the ECU. When this switch
goes low a engine stop will be sent. A value of 0 disables the starter
control.
## EFI_SP_GEN_FN
This is the RC option (auxiliary function) to use for generator
control. This should be set to one of the scripting RC options (from
300 to 307) if generator control is needed. Then an RCn_OPTION should
be set to the same value. When this switch goes high the generator
start function will be sent to the ECU. When this switch goes low a
generator stop will be sent. A value of 0 disables the generator
control.
## EFI_SP_MIN_RPM
This is the minimum running RPM. When set to a positive value then the
driver will monitor engine RPM when the engine is started and if it
drops below this value then an engine start will be sent to restart
the engine.
# 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
- EFI_TYPE should be set to 7
then the flight controller should rebooted and parameters should be
refreshed.
Once loaded the EFI_SP parameters will appear and should be set
according to the parameter list above.
A 2 position RC switch should be setup with RCn_OPTION=300 (or the
value of EFI_SP_START_FN) to enable starter control. When that switch
goes high the engine will be started. When it goes low the engine will
be stopped.
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.
Setting EFI_SP_RPM_MIN allows for automatic in-flight engine
restart. If the engine RPM drops below this EFI_SP_RPM_MIN for 2
seconds while the engine should be started then an engine start
command will be sent to restart the engine.