diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_efi.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_efi.lua index e19f963051..460fa50e71 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_efi.lua +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_efi.lua @@ -1,5 +1,5 @@ --[[ -Name: EFI Scripting backend driver for simulated engine +EFI Scripting backend driver for simulated engine --]] local SCRIPT_NAME = "EFI: SITL" @@ -19,10 +19,10 @@ local function bind_add_param(name, idx, default_value) return bind_param(PARAM_TABLE_PREFIX .. name) end --- Setup EFI Parameters -PARAM_TABLE_PREFIX = "SIM_EFI_" +-- Set up EFI parameters +PARAM_TABLE_PREFIX = 'SIM_EFI_' PARAM_TABLE_KEY = 36 -assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add SIM_EFI param table') +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 1), 'could not add ' .. string.sub(PARAM_TABLE_PREFIX, 1, -2) .. ' param table') --[[ // @Param: SIM_EFI_CHT1_INC // @DisplayName: CHT1 Increase diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_escs.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_escs.lua new file mode 100644 index 0000000000..c839a272e9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/sitl_escs.lua @@ -0,0 +1,137 @@ +--[[ +ESC telemetry simulator for SITL, with support for failures +--]] + +local SCRIPT_NAME = "ESC: SITL" +local UPDATE_HZ = 50 +-- Ensure the script is loaded in SITL only +assert(param:get('SIM_OPOS_LAT') ~= nil, string.format('%s was designed for SITL', SCRIPT_NAME)) + +-- Bind parameter utilities +local function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +local 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 + +-- Set up ESC parameters +local PARAM_TABLE_KEY = 20 +local PARAM_TABLE_PREFIX = 'SIM_ESC_' +assert(false or param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 1), 'could not add ' .. string.sub(PARAM_TABLE_PREFIX, 1, -2) .. ' param table') + +--[[ + // @Param: SIM_ESC_TLM_FAIL + // @DisplayName: Simulated ESC telemetry failure mask + // @Description: Telemetry will stop being sent for the ESCs with the corresponding bits set in this mask + // @Bitmask: 0:ESC1, 1:ESC2, 2:ESC3, 3:ESC4, 4:ESC5, 5:ESC6, 6:ESC7, 7:ESC8 +--]] +local TELEM_FAIL_MASK = bind_add_param('TLM_FAIL', 1, 0) + +-- Table of ESCs detected in our configuration parameters +local ESCs = {} +-- Constructor to return an ESC information table +local function new_esc_info(esc_index, servo_index) + local self = {} + self.esc_index = esc_index + self.servo_index = servo_index + self.throttle = 0 + return self +end + +-- Class for undoing ArduPilot's thrust curve linearization +function ThrustLinearization(curve_expo, spin_min, spin_max, batt_voltage_min, batt_voltage_max) + -- Create a table to hold the object's state + local self = { + curve_expo = curve_expo or 0.65, -- Thrust curve exponent + spin_min = spin_min or 0.15, -- Minimum actuator output + spin_max = spin_max or 0.95, -- Maximum actuator output + batt_voltage_min = batt_voltage_min or 0, -- Battery voltage compensation minimum + batt_voltage_max = batt_voltage_max or 0, -- Battery voltage compensation maximum + lift_max = 1.0 -- Default lift_max + } + + local function constrain(val, min, max) + if val < min then return min end + if val > max then return max end + return val + end + + local function remove_thrust_curve_and_volt_scaling(throttle, voltage) + local battery_scale = 1.0 + if voltage and voltage > 0 then + battery_scale = 1.0 / voltage + end + + local thrust_curve_expo = constrain(self.curve_expo, -1.0, 1.0) + if math.abs(thrust_curve_expo) < 1e-6 then + -- Linear case + return throttle / (self.lift_max * battery_scale) + end + + -- Inverse curve + local thrust = (throttle / battery_scale) * (2.0 * thrust_curve_expo) - (thrust_curve_expo - 1.0) + thrust = (thrust * thrust) - ((1.0 - thrust_curve_expo) * (1.0 - thrust_curve_expo)) + thrust = thrust / (4.0 * thrust_curve_expo * self.lift_max) + + return constrain(thrust, 0.0, 1.0) + end + + -- Method to calculate thrust from actuator input + function self.actuator_to_thrust(actuator, voltage) + -- Normalize actuator value + local normalized_actuator = (actuator - self.spin_min) / (self.spin_max - self.spin_min) + return constrain(remove_thrust_curve_and_volt_scaling(normalized_actuator, voltage), 0.0, 1.0) + end + + return self +end + +local function setup() + for can_idx = 1, 2 do + local esc_mask = param:get('CAN_D' .. can_idx .. '_UC_ESC_BM') + local esc_offset = param:get('CAN_D' .. can_idx .. '_UC_ESC_OF') + assert(esc_mask, 'could not find CAN_D' .. can_idx .. '_UC_ESC_BM parameter') + assert(esc_offset, 'could not find CAN_D' .. can_idx .. '_UC_ESC_OF parameter') + for i = 0, 31 do + if esc_mask & (1 << i) ~= 0 then + -- Currently, the esc index is the same as the servo index, but + -- we're planning to make it possible to make them different + ESCs[#ESCs + 1] = new_esc_info(esc_offset + i, esc_offset + i) + end + end + end +end + + +local function update() + -- Get the voltage and current from batt1 + local voltage = battery:voltage(0) or 0 + local current = battery:current_amps(0) or 0 + + -- Sum of the throttle squared for each ESC + local total_current_factor = 0 + for esc in ESCs do + + end +end + +-- Wrapper to handle errors +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, "Internal Error: " .. err) + return protected_wrapper, 1000 + end + return protected_wrapper, 1000 / UPDATE_HZ +end + +-- Set up the script +setup() + +-- Start the update loop +return protected_wrapper()