WIP: escs

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

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()