From 8cb638b7e1ad69ba50c78c6c74e5d8765292c90e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 May 2023 17:21:52 +1000 Subject: [PATCH] AP_Scripting: added Halo6000 generator/EFI driver used to monitor Foxtech Halo6000 generator for Thor210 hexacopter --- .../AP_Scripting/drivers/EFI_Halo6000.lua | 338 ++++++++++++++++++ .../AP_Scripting/drivers/EFI_Halo6000.md | 50 +++ .../AP_Scripting/examples/EFI_tester.lua | 29 ++ 3 files changed, 417 insertions(+) create mode 100644 libraries/AP_Scripting/drivers/EFI_Halo6000.lua create mode 100644 libraries/AP_Scripting/drivers/EFI_Halo6000.md diff --git a/libraries/AP_Scripting/drivers/EFI_Halo6000.lua b/libraries/AP_Scripting/drivers/EFI_Halo6000.lua new file mode 100644 index 0000000000..20c7c94504 --- /dev/null +++ b/libraries/AP_Scripting/drivers/EFI_Halo6000.lua @@ -0,0 +1,338 @@ +--[[ + EFI Scripting backend driver for Halo6000 generator +--]] + +-- Check Script uses a miniumum firmware version +local SCRIPT_AP_VERSION = 4.3 +local SCRIPT_NAME = "EFI: Halo6000 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 = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 40 +PARAM_TABLE_PREFIX = "EFI_H6K_" + +UPDATE_RATE_HZ = 40 + +-- 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_uint8(frame, ofs) + return frame:data(ofs) +end + +function get_uint16(frame, ofs) + return frame:data(ofs) + (frame:data(ofs+1) << 8) +end + +function constrain(v, vmin, vmax) + return math.max(math.min(v,vmax),vmin) +end + +local efi_backend = nil + +-- Setup EFI Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'could not add EFI_H6K param table') + +--[[ + // @Param: EFI_H6K_ENABLE + // @DisplayName: Enable Halo6000 EFI driver + // @Description: Enable Halo6000 EFI driver + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local EFI_H6K_ENABLE = bind_add_param('ENABLE', 1, 0) + +--[[ + // @Param: EFI_H6K_CANDRV + // @DisplayName: Halo6000 CAN driver + // @Description: Halo6000 CAN driver. Use 1 for first CAN scripting driver, 2 for 2nd driver + // @Values: 0:Disabled,1:FirstCAN,2:SecondCAN + // @User: Standard +--]] +local EFI_H6K_CANDRV = bind_add_param('CANDRV', 2, 0) -- CAN driver number + +--[[ + // @Param: EFI_H6K_START_FN + // @DisplayName: Halo6000 start auxilliary function + // @Description: The RC auxilliary function number for start/stop of the generator. Zero to disable start function + // @Values: 0:Disabled,300:300,301:301,302:302,303:303,304:304,305:305,306:306,307:307 + // @User: Standard +--]] +local EFI_H6K_START_FN = bind_add_param('START_FN', 3, 0) + +--[[ + // @Param: EFI_H6K_TELEM_RT + // @DisplayName: Halo6000 telemetry rate + // @Description: The rate that additional generator telemetry is sent + // @Units: Hz + // @User: Standard +--]] +local EFI_H6K_TELEM_RT = bind_add_param('TELEM_RT', 4, 2) + +--[[ + // @Param: EFI_H6K_FUELTOT + // @DisplayName: Halo6000 total fuel capacity + // @Description: The capacity of the tank in litres + // @Units: litres + // @User: Standard +--]] +local EFI_H6K_FUELTOT = bind_add_param('FUELTOT', 5, 20) + +if EFI_H6K_ENABLE:get() == 0 then + return +end + +-- Register for the CAN drivers +local CAN_BUF_LEN = 25 +if EFI_H6K_CANDRV:get() == 1 then + gcs:send_text(0, string.format("EFI_H6K: attaching to CAN1")) + driver1 = CAN.get_device(CAN_BUF_LEN) +elseif EFI_H6K_CANDRV:get() == 2 then + gcs:send_text(0, string.format("EFI_H6K: attaching to CAN2")) + driver1 = CAN.get_device2(CAN_BUF_LEN) +end + +if not driver1 then + gcs:send_text(0, string.format("EFI_H6K: Failed to load driver")) + return +end + + +--[[ + EFI Engine Object +--]] +local function engine_control() + 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 throttle_pos = 0 + local fuel_pct = 0 + local cyl_temp = 0 + local cool_temp = 0 + local out_volt = 0 + local out_current = 0 + local last_state_update_t = 0 + local last_rpm_t = 0 + local last_telem_update_t = 0 + local last_stop_message_t = 0 + local C_TO_KELVIN = 273.2 + local engine_started = false + + -- read telemetry packets + function self.update_telemetry() + local max_packets = 25 + local count = 0 + while count < max_packets do + frame = driver1:read_frame() + count = count + 1 + if not frame then + break + end + + -- All Frame IDs for this EFI Engine are in the 11-bit address space + if not frame:isExtended() then + self.handle_packet(frame) + end + 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 + function self.handle_packet(frame) + local id = frame:id_signed() + if id == 0x1c1 then + -- 20Hz telem + rpm = get_uint16(frame, 0) + last_rpm_t = get_time_sec() + throttle_pos = get_uint16(frame, 2)*0.1 + fuel_pct = get_uint8(frame, 4)*0.5 + cyl_temp = get_uint8(frame, 5) + cool_temp = get_uint8(frame, 6) + elseif id == 0x1c2 then + -- 20Hz telem2 + out_volt = get_uint16(frame, 0)*0.2 + out_current = get_uint16(frame, 2)*0.2 - 200.0 + end + end + + -- Build and set the EFI_State that is passed into the EFI Scripting backend + function self.set_EFI_State() + -- Cylinder_Status + cylinder_state:cylinder_head_temperature(cyl_temp + C_TO_KELVIN) + cylinder_state:exhaust_gas_temperature(cool_temp + C_TO_KELVIN) + efi_state:engine_speed_rpm(uint32_t(rpm)) + efi_state:intake_manifold_temperature(cool_temp + C_TO_KELVIN) + efi_state:throttle_position_percent(math.floor(throttle_pos)) + efi_state:ignition_voltage(out_volt) + efi_state:estimated_consumed_fuel_volume_cm3((100.0-fuel_pct)*0.01*EFI_H6K_FUELTOT:get()*1000) + + -- copy cylinder_state to efi_state + efi_state:cylinder_status(cylinder_state) + + efi_state:last_updated_ms(millis()) + + -- Set the EFI_State into the EFI scripting driver + efi_backend:handle_scripting(efi_state) + end + + -- update telemetry output for extra telemetry values + function self.update_telem_out() + local rate = EFI_H6K_TELEM_RT:get() + if rate <= 0 then + return + end + local now = get_time_sec() + if now - last_telem_update_t < 1.0 / rate then + return + end + last_telem_update_t = now + gcs:send_named_float('H6K_FUEL', fuel_pct) + gcs:send_named_float('H6K_CYLT', cyl_temp) + gcs:send_named_float('H6K_COOLT', cool_temp) + gcs:send_named_float('H6K_AMPS', out_current) + end + + -- send a frame with checksum + function self.write_frame_checksum(msg) + local sum = 0 + for i=0, 7 do + sum = sum + msg:data(i) + end + sum = sum % 0xff + msg:data(7, sum) + msg:dlc(8) + driver1:write_frame(msg, 10000) + end + + -- send an engine start command + function self.send_engine_start() + local msg = CANFrame() + msg:id(0x1A0) + msg:data(0,1) + self.write_frame_checksum(msg) + end + + -- send an engine stop command + function self.send_engine_stop() + local msg = CANFrame() + msg:id(0x1A0) + msg:data(0,0) + self.write_frame_checksum(msg) + 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("EFIH6K: stopping generator")) + end + end + + -- update starter control + function self.update_starter() + local start_fn = EFI_H6K_START_FN:get() + if start_fn == 0 then + 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 + 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("EFIH6K: starting generator")) + engine_start_t = get_time_sec() + self.send_engine_start() + should_be_running = true + end + if start_state > 0 and engine_started then + should_be_running = true + end + local min_rpm = 500 + 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("EFIH6K: re-starting generator")) + 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 + + -- return the instance + return self +end -- end function engine_control(_driver) + +local engine1 = engine_control() + +function update() + 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() + engine1.update_telem_out() + engine1.update_starter() +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 / UPDATE_RATE_HZ +end + +-- start running update loop +return protected_wrapper() diff --git a/libraries/AP_Scripting/drivers/EFI_Halo6000.md b/libraries/AP_Scripting/drivers/EFI_Halo6000.md new file mode 100644 index 0000000000..d27d304c15 --- /dev/null +++ b/libraries/AP_Scripting/drivers/EFI_Halo6000.md @@ -0,0 +1,50 @@ +# EFI Halo6000 Driver + +This driver implements support for the Halo6000 generator for hybrid +multicopters, using CAN protocol. + +# Parameters + +The script used the following parameters: + +## EFI_H6K_ENABLE + +this must be set to 1 to enable the driver + +## EFI_H6K_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_H6K_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 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 starter +control. + +## EFI_H6K_TELEM_RT + +This is the rate in Hz at which NAMED_VALUE_FLOAT messages are used to +send additional telemetry data to the GCS for display to the operator. + +# 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_H6K parameters will appear and should be set +according to the parameter list above. + +The GCS will receive EFI_STATUS MAVLink messages which includes RPM, +and temperatures. diff --git a/libraries/AP_Scripting/examples/EFI_tester.lua b/libraries/AP_Scripting/examples/EFI_tester.lua index f5f95bf575..0bb2719d85 100644 --- a/libraries/AP_Scripting/examples/EFI_tester.lua +++ b/libraries/AP_Scripting/examples/EFI_tester.lua @@ -236,11 +236,40 @@ function send_HFE(driver) end +--[[ + send Halo6000 data. Called at 100Hz +--]] +function send_Halo6K(driver) + + --local msg = CANFrame() + local t = get_time_sec() + + local RPM = 1200 + math.floor(1000*math.sin(t)) + rev_counter = rev_counter + (RPM/60.0)*0.01 + + -- fast telem1 + local msg = CANFrame() + msg:id(0x1c1) + put_u16_LE(msg,0,RPM) + msg:dlc(8) + driver:write_frame(msg, 10000) + + -- fast telem2 + msg = CANFrame() + msg:id(0x1c2) + put_u16_LE(msg,0,math.floor(30.2*5)) + put_u16_LE(msg,2,math.floor(3.2*5)+200) + msg:dlc(8) + driver:write_frame(msg, 10000) +end + function update() if EFISIM_TYPE:get() == 1 then send_SkyPower(driver2) elseif EFISIM_TYPE:get() == 2 then send_HFE(driver2) + elseif EFISIM_TYPE:get() == 3 then + send_Halo6K(driver2) end return update, math.floor(1000/EFISIM_RATE_HZ:get()) end