diff --git a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua index 2c4a85992a..a8873604ea 100644 --- a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua +++ b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua @@ -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")) diff --git a/libraries/AP_Scripting/examples/EFI_tester.lua b/libraries/AP_Scripting/examples/EFI_tester.lua index 0d44bee207..f5f95bf575 100644 --- a/libraries/AP_Scripting/examples/EFI_tester.lua +++ b/libraries/AP_Scripting/examples/EFI_tester.lua @@ -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)