Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: added custom telem msgs for EFI_Skypower driver 

also:
- and added parameter for enable/disable start when disarmed
- added logging for extra fields not in EFI logging
- fixed endianness of EFI tester
This commit is contained in:
Andrew Tridgell 2023-01-11 10:30:09 +11:00
parent 1d70180809
commit 29f124543b
2 changed files with 142 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,16 +68,19 @@ local LITRES_TO_LBS = 1.6095 -- 6.1 lbs of fuel per gallon -> 1.6095
local efi_backend = nil
-- Setup EFI Parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add EFI_SP param table')
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add EFI_SP param table')
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_THR_RATE = bind_add_param('THR_RATE', 5, 50) -- 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
local EFI_SP_MIN_RPM = bind_add_param('MIN_RPM', 8, 100) -- min RPM, for engine restart
local EFI_SP_TLM_RT = bind_add_param('TLM_RT', 9, 0) -- rate for extra telemetry values
local EFI_SP_LOG_RT = bind_add_param('LOG_RT', 10, 10) -- rate for logging
local EFI_SP_ST_DISARM = bind_add_param('ST_DISARM', 11, 0) -- allow start when disarmed
if EFI_SP_ENABLE:get() == 0 then
gcs:send_text(0, string.format("EFISP: disabled"))
@ -106,6 +109,13 @@ function C_TO_KELVIN(temp)
return temp + 273.15
end
--[[
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
--[[
EFI Engine Object
--]]
@ -137,9 +147,13 @@ local function engine_control(_driver, _idx)
local last_rpm_t = get_time_sec()
local last_state_update_t = get_time_sec()
local last_thr_update = get_time_sec()
local last_telem_update = get_time_sec()
local last_log_t = get_time_sec()
local last_stop_message_t = get_time_sec()
local engine_started = false
local generator_started = false
local engine_start_t = 0.0
local last_throttle = 0.0
-- frames for sending commands
local FRM_500 = uint32_t(0x500)
@ -157,6 +171,7 @@ local function engine_control(_driver, _idx)
temps.egt = 0.0 -- Exhaust Gas Temperature
temps.cht = 0.0 -- Cylinder Head Temperature
temps.imt = 0.0 -- intake manifold temperature
temps.oilt = 0.0 -- oil temperature
-- read telemetry packets
function self.update_telemetry()
@ -169,7 +184,7 @@ local function engine_control(_driver, _idx)
break
end
-- All Frame IDs for this EFI Engine are in the 11-bit address space
-- All Frame IDs for this EFI Engine are extended
if frame:isExtended() then
self.handle_EFI_packet(frame, _idx)
end
@ -211,6 +226,7 @@ local function engine_control(_driver, _idx)
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
end
end
@ -223,7 +239,7 @@ local function engine_control(_driver, _idx)
cylinder_state:injection_time_ms(inj_time*0.001)
efi_state:engine_speed_rpm(uint32_t(rpm))
efi_state:engine_load_percent(current_load)
efi_state:engine_load_percent(math.floor(current_load))
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)
@ -231,6 +247,7 @@ local function engine_control(_driver, _idx)
efi_state:atmospheric_pressure_kpa(air_pressure*0.1)
efi_state:ignition_voltage(supply_voltage)
efi_state:intake_manifold_temperature(C_TO_KELVIN(temps.imt))
efi_state:throttle_out(last_throttle * 100)
-- copy cylinder_state to efi_state
efi_state:cylinder_status(cylinder_state)
@ -245,6 +262,7 @@ local function engine_control(_driver, _idx)
--- send throttle command, thr is 0 to 1
function self.send_throttle(thr)
last_throttle = thr
local msg = CANFrame()
msg:id(FRM_500)
msg:data(0,1)
@ -272,6 +290,11 @@ local function engine_control(_driver, _idx)
msg:data(7,10)
msg:dlc(8)
driver:write_frame(msg, 10000)
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
end
-- start generator
@ -299,28 +322,43 @@ local function engine_control(_driver, _idx)
return
end
local start_state = rc:get_aux_cached(start_fn)
if start_state == nil then
start_state = 0
end
local should_be_running = false
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
if start_state == 2 and not engine_started and allow_run_engine() then
engine_started = true
gcs:send_text(0, string.format("EFISP: starting engine"))
engine_start_t = get_time_sec()
self.send_engine_start()
should_be_running = true
end
if start_state > 0 and engine_started and allow_run_engine() then
should_be_running = true
end
local min_rpm = EFI_SP_MIN_RPM:get()
if min_rpm > 0 and engine_started and rpm < min_rpm then
if min_rpm > 0 and engine_started and rpm < min_rpm and allow_run_engine() 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()
engine_start_t = get_time_sec()
end
end
--[[
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
end
-- update generator control
@ -359,6 +397,38 @@ local function engine_control(_driver, _idx)
end
self.send_throttle(thr)
end
-- 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)
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
logger.write('EFSP','Thr,CLoad,TLoad,OilT,RPM,gRPM,gAmp,gCur', 'ffffffff',
last_throttle, current_load, target_load, temps.oilt, rpm,
gen.rpm, gen.amps, gen.batt_current)
end
-- return the instance
return self
@ -383,6 +453,8 @@ function update()
engine1.update_starter()
engine1.update_generator()
engine1.update_throttle()
engine1.update_telem_out()
engine1.update_logging()
end
gcs:send_text(0, SCRIPT_NAME .. string.format(" loaded"))

103
libraries/AP_Scripting/examples/EFI_tester.lua
View File

@ -12,10 +12,17 @@ end
local PARAM_TABLE_KEY = 13
local PARAM_TABLE_PREFIX = "EFISIM_"
-- 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 Parameter(PARAM_TABLE_PREFIX .. name)
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
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table')
@ -27,36 +34,48 @@ function get_time_sec()
return millis():tofloat() * 0.001
end
local FRM_100 = uint32_t(0x100)
local FRM_101 = uint32_t(0x101)
local FRM_102 = uint32_t(0x102)
local FRM_104 = uint32_t(0x104)
local FRM_105 = uint32_t(0x105)
local FRM_106 = uint32_t(0x106)
local FRM_10A = uint32_t(0x10A)
local FRM_10C = uint32_t(0x10C)
local FRM_10D = uint32_t(0x10D)
local FRM_10F = uint32_t(0x10F)
local FRM_113 = uint32_t(0x113)
local FRM_114 = uint32_t(0x114)
local FRM_115 = uint32_t(0x115)
local FRM_100 = uint32_t(0x80000100)
local FRM_101 = uint32_t(0x80000101)
local FRM_102 = uint32_t(0x80000102)
local FRM_104 = uint32_t(0x80000104)
local FRM_105 = uint32_t(0x80000105)
local FRM_106 = uint32_t(0x80000106)
local FRM_10A = uint32_t(0x8000010A)
local FRM_10C = uint32_t(0x8000010C)
local FRM_10D = uint32_t(0x8000010D)
local FRM_10F = uint32_t(0x8000010F)
local FRM_113 = uint32_t(0x80000113)
local FRM_114 = uint32_t(0x80000114)
local FRM_115 = uint32_t(0x80000115)
function put_u8(msg, ofs, v)
msg:data(ofs,v&0xFF)
end
function put_u16(msg, ofs, v)
function put_u16_LE(msg, ofs, v)
msg:data(ofs,v&0xFF)
msg:data(ofs+1,v>>8)
end
function put_u32(msg, ofs, v)
function put_u32_LE(msg, ofs, v)
msg:data(ofs+0,v&0xFF)
msg:data(ofs+1,(v>>8)&0xFF)
msg:data(ofs+2,(v>>16)&0xFF)
msg:data(ofs+3,(v>>24)&0xFF)
end
function put_u16_BE(msg, ofs, v)
msg:data(ofs+1,v&0xFF)
msg:data(ofs,v>>8)
end
function put_u32_BE(msg, ofs, v)
msg:data(ofs+3,v&0xFF)
msg:data(ofs+2,(v>>8)&0xFF)
msg:data(ofs+1,(v>>16)&0xFF)
msg:data(ofs+0,(v>>24)&0xFF)
end
local rev_counter = 0
--[[
@ -73,102 +92,102 @@ function send_SkyPower(driver)
-- 0x100
local msg = CANFrame()
msg:id(FRM_100)
put_u16(msg,0,RPM)
put_u16(msg,2,13*10) -- ignition angle
put_u16(msg,4,45*10) -- throttle angle
put_u16_LE(msg,0,RPM)
put_u16_LE(msg,2,13*10) -- ignition angle
put_u16_LE(msg,4,45*10) -- throttle angle
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x101
msg = CANFrame()
msg:id(FRM_101)
put_u16(msg,2,917) -- air pressure
put_u16_LE(msg,2,917) -- air pressure
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x102
msg = CANFrame()
msg:id(FRM_102)
put_u16(msg,0,7*10) -- ingition gap
put_u16(msg,2,270*10) -- injection angle
put_u16(msg,4,37000) -- injection time
put_u16_LE(msg,0,7*10) -- ingition gap
put_u16_LE(msg,2,270*10) -- injection angle
put_u16_LE(msg,4,37000) -- injection time
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x104
msg = CANFrame()
msg:id(FRM_104)
put_u16(msg,0,math.floor(14.8*10)) -- supply voltage
put_u16_LE(msg,0,math.floor(14.8*10)) -- supply voltage
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x105
msg = CANFrame()
msg:id(FRM_105)
put_u16(msg,0,172*10) -- engine temp head 1
put_u16(msg,2,65*10) -- air temp
put_u16(msg,4,320*10) -- exhaust temp
put_u16(msg,6,113*10) -- ecu temp
put_u16_LE(msg,0,172*10) -- engine temp head 1
put_u16_LE(msg,2,65*10) -- air temp
put_u16_LE(msg,4,320*10) -- exhaust temp
put_u16_LE(msg,6,113*10) -- ecu temp
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x106
msg = CANFrame()
msg:id(FRM_106)
put_u32(msg,0,math.floor(rev_counter))
put_u32_LE(msg,0,math.floor(rev_counter))
put_u8(msg,4,math.floor(t))
put_u8(msg,5,math.floor(t/60))
put_u16(msg,6,math.floor(t/3600))
put_u16_LE(msg,6,math.floor(t/3600))
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x10A
msg = CANFrame()
msg:id(FRM_10A)
put_u16(msg,6,72*10) -- target load
put_u16_LE(msg,6,72*10) -- target load
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x10C
msg = CANFrame()
msg:id(FRM_10C)
put_u16(msg,4,145*10) -- engine head temp 2
put_u16(msg,6,315*10) -- exhaust temp 2
put_u16_LE(msg,4,145*10) -- engine head temp 2
put_u16_LE(msg,6,315*10) -- exhaust temp 2
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x10D
msg = CANFrame()
msg:id(FRM_10D)
put_u16(msg,4,72*10) -- current load
put_u16_LE(msg,4,72*10) -- current load
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x10F
msg = CANFrame()
msg:id(FRM_10F)
put_u16(msg,4,2*10) -- fuel consumption
put_u16_LE(msg,4,2*10) -- fuel consumption
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x113
msg = CANFrame()
msg:id(FRM_113)
put_u16(msg,2,1*10) -- gen amps
put_u16_LE(msg,2,1*10) -- gen amps
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x114
msg = CANFrame()
msg:id(FRM_114)
put_u16(msg,2,2400*10) -- gen RPM
put_u16_LE(msg,2,2400*10) -- gen RPM
msg:dlc(8)
driver:write_frame(msg, 10000)
-- 0x115
msg = CANFrame()
msg:id(FRM_115)
put_u16(msg,4,30*100) -- gen current
put_u16_LE(msg,4,30*100) -- gen current
msg:dlc(8)
driver:write_frame(msg, 10000)
end
@ -188,14 +207,14 @@ function send_HFE(driver)
local msg = CANFrame()
local base_id = uint32_t(0x88000005)
msg:id(base_id | 0x00000)
put_u16(msg,1,RPM)
put_u16_BE(msg,1,RPM)
msg:dlc(8)
driver:write_frame(msg, 10000)
-- slow telem0
msg = CANFrame()
msg:id(base_id | 0x10000)
put_u16(msg,5,101324/2) -- air pressure
put_u16_BE(msg,5,101324/2) -- air pressure
msg:dlc(8)
driver:write_frame(msg, 10000)
@ -210,7 +229,7 @@ function send_HFE(driver)
msg = CANFrame()
msg:id(base_id | 0x30000)
put_u8(msg,1,math.floor((67+128)/1.5)) -- MAT
put_u16(msg,6,173) -- fuel flow in grams/hr
put_u16_BE(msg,6,173) -- fuel flow in grams/hr
msg:dlc(8)
driver:write_frame(msg, 10000)