From 2538490c60a4b637371fc73744def5c3f9087b65 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Oct 2022 09:02:01 +1100 Subject: [PATCH] AP_Scripting: added throttle and generator control for EFI_SkyPower driver and added documentation for the driver --- .../AP_Scripting/drivers/EFI_SkyPower.lua | 170 ++++++++++++++++-- .../AP_Scripting/drivers/EFI_SkyPower.md | 91 ++++++++++ 2 files changed, 249 insertions(+), 12 deletions(-) create mode 100644 libraries/AP_Scripting/drivers/EFI_SkyPower.md diff --git a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua index 75d8062600..0fd7cbd384 100644 --- a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua +++ b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua @@ -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")) diff --git a/libraries/AP_Scripting/drivers/EFI_SkyPower.md b/libraries/AP_Scripting/drivers/EFI_SkyPower.md new file mode 100644 index 0000000000..26aa004bdc --- /dev/null +++ b/libraries/AP_Scripting/drivers/EFI_SkyPower.md @@ -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.