diff --git a/libraries/AP_Scripting/applets/x-quad-cg-allocation.lua b/libraries/AP_Scripting/applets/x-quad-cg-allocation.lua new file mode 100644 index 0000000000000..89d80e988318d --- /dev/null +++ b/libraries/AP_Scripting/applets/x-quad-cg-allocation.lua @@ -0,0 +1,283 @@ +-- x-quad-cg-allocation.lua: Adjust the control allocation matrix for offset CoG. +-- +-- WARNING: This script is applicable only to X-type quadrotors and quadplanes. +-- +-- How To Use +-- 1. Place this script in the "scripts" directory. +-- 2. Set FRAME_CLASS or Q_FRAME_CLASS to 17 to enable the dynamic scriptable mixer. +-- 3. Enable Lua scripting via the SCR_ENABLE parameter. +-- 4. Reboot. +-- 5. Fly the vehicle. +-- 6. Adjust the value of the CGA_RATIO parameter. +-- +-- How It Works +-- 1. The control allocation matrix is adjusted for thrust and pitch based on the ??? parameter value. + +--[[ +Global definitions. +--]] +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local SCRIPT_NAME = "CoG adjust script" +local LOOP_RATE_HZ = 10 +local last_warning_time_ms = uint32_t() -- Time we last sent a warning message to the user. +local WARNING_DEADTIME_MS = 1000 -- How often the user should be warned. +local is_mixer_matrix_static = false +local is_mixer_matrix_dynamic = false +local last_ratio = 1 + +-- State machine states. +local FSM_STATE = { + INACTIVE = 0, + INITIALIZE = 1, + ACTIVE = 2, + FINISHED = 3 +} +local current_state = FSM_STATE.INACTIVE +local next_state = FSM_STATE.INACTIVE + + +--[[ +New parameter declarations +--]] +local PARAM_TABLE_KEY = 139 +local PARAM_TABLE_PREFIX = "CGA_" + +-- Bind a parameter to a variable. +function bind_param(name) + return Parameter(name) +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 + +-- Add param table. +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 1), SCRIPT_NAME .. ': Could not add param table.') + +--[[ + // @Param: CGA_RATIO + // @DisplayName: CoG adjustment ratio + // @Description: The ratio between the front and back motor outputs during steady-state hover. Positive when the CoG is in front of the motors midpoint (front motors work harder). + // @Range: 0.5 2 + // @User: Advanced +--]] +CGA_RATIO = bind_add_param('RATIO', 1, 1) + +-- Bindings to existing parameters + +--[[ +Potential additions: +--]] +-- Warn the user, throttling the message rate. +function warn_user(msg, severity) + severity = severity or MAV_SEVERITY.WARNING -- Optional severity argument. + if millis() - last_warning_time_ms > WARNING_DEADTIME_MS then + gcs:send_text(severity, SCRIPT_NAME .. ": " .. msg) + last_warning_time_ms = millis() + end +end + +-- Decide if the given ratio value makes sense. +function sanitize_ratio(ratio) + if (ratio < 0.5) or (ratio > 2) then + warn_user("CGA_RATIO value out of bounds.", MAV_SEVERITY.ERROR) + CGA_RATIO:set(1.0) + return 1.0 -- Return default. + else + return ratio + end +end + +-- Normalize the throttle factors to max 1 +function normalize_throttle(factors) + -- Find maximum value. + local max_factor = 0 + for _, factor in ipairs(factors) do + max_factor = math.max(max_factor, factor) + end + -- Adjust all values by it. + normalized_factors = {} + for _, factor in ipairs(factors) do + table.insert(normalized_factors, factor / max_factor) + end + return normalized_factors +end + +-- Calculate the thrust factors given a ratio. +function build_factors(ratio) + local r1 = 2.0/(1+ratio) + local r2 = 2.0*ratio/(1+ratio) + local quad_x_factors = {r2, r1, r2, r1} + return normalize_throttle(quad_x_factors) +end + +-- Adjust the dynamic motor mixer. +function update_dynamic_mixer(ratio) + + Motors_dynamic:add_motor(0, 1) + Motors_dynamic:add_motor(1, 3) + Motors_dynamic:add_motor(2, 4) + Motors_dynamic:add_motor(3, 2) + + factors = motor_factor_table() + + -- Roll stays as-is. + factors:roll(0, -0.5) + factors:roll(1, 0.5) + factors:roll(2, 0.5) + factors:roll(3, -0.5) + + -- Pitch stays as-is. + factors:pitch(0, 0.5) + factors:pitch(1, -0.5) + factors:pitch(2, 0.5) + factors:pitch(3, -0.5) + + -- Yaw stays as-is. + factors:yaw(0, 0.5) + factors:yaw(1, 0.5) + factors:yaw(2, -0.5) + factors:yaw(3, -0.5) + + -- Throttle is modulated. + throttle_factors = build_factors(ratio) + factors:throttle(0, throttle_factors[1]) + factors:throttle(1, throttle_factors[2]) + factors:throttle(2, throttle_factors[3]) + factors:throttle(3, throttle_factors[4]) + + Motors_dynamic:load_factors(factors) + + if not Motors_dynamic:init(4) then + warn_user("Failed to initialize motor matrix!", MAV_SEVERITY.EMERGENCY) + else + if ratio ~= last_ratio then + warn_user("Set ratio to " .. tostring(ratio), MAV_SEVERITY.INFO) + last_ratio = ratio + end + end + motors:set_frame_string("Dynamic CoM adjust") + +end + +-- Adjust the static motor mixer. +function update_static_mixer(ratio) + MotorsMatrix:add_motor_raw(0,-0.5, 0.5, 0.5, 2) + MotorsMatrix:add_motor_raw(1, 0.5,-0.5, 0.5, 4) + MotorsMatrix:add_motor_raw(2, 0.5, 0.5,-0.5, 1) + MotorsMatrix:add_motor_raw(3,-0.5,-0.5,-0.5, 3) + + throttle_factors = build_factors(ratio) + MotorsMatrix:set_throttle_factor(0, throttle_factors[1]) + MotorsMatrix:set_throttle_factor(1, throttle_factors[2]) + MotorsMatrix:set_throttle_factor(2, throttle_factors[3]) + MotorsMatrix:set_throttle_factor(3, throttle_factors[4]) + + if not MotorsMatrix:init(4) then + warn_user("Failed to initialize motor matrix!", MAV_SEVERITY.EMERGENCY) + else + if ratio ~= last_ratio then + warn_user("Set ratio to " .. tostring(ratio), MAV_SEVERITY.INFO) + last_ratio = ratio + end + end + motors:set_frame_string("Static CoM adjust") +end + +-- Decide if the UA is a Quad X quadplane. +function inspect_frame_class_fw() + + Q_ENABLE = bind_param("Q_ENABLE") + Q_FRAME_CLASS = bind_param("Q_FRAME_CLASS") + + if FWVersion:type()==3 then + -- Test for the validity of the parameters. + if Q_ENABLE:get()==1 then + if Q_FRAME_CLASS:get()==15 then + is_mixer_matrix_static = true + elseif Q_FRAME_CLASS:get()==17 then + is_mixer_matrix_dynamic = true + end + end + end +end + +-- Decide if the UA is a Quad X multicopter. +function inspect_frame_class_mc() + + FRAME_CLASS = bind_param("FRAME_CLASS") + + if FWVersion:type()==2 then + if FRAME_CLASS:get()==15 then + is_mixer_matrix_static = true + elseif FRAME_CLASS:get()==17 then + is_mixer_matrix_dynamic = true + end + end +end + +--[[ +Activation conditions +--]] +-- Check for script activating conditions here. +-- Check frame types. +function can_start() + result = is_mixer_matrix_static or is_mixer_matrix_dynamic + return result +end + +--[[ +State machine +--]] +function fsm_step() + if current_state == FSM_STATE.INACTIVE then + if can_start() then + next_state = FSM_STATE.INITIALIZE + else + next_state = FSM_STATE.FINISHED + warn_user("Could not find scriptable mixer", MAV_SEVERITY.ERROR) + end + + elseif current_state == FSM_STATE.INITIALIZE then + if is_mixer_matrix_static then + local ratio = sanitize_ratio(CGA_RATIO:get()) + update_static_mixer(ratio) + next_state = FSM_STATE.FINISHED + else + next_state = FSM_STATE.ACTIVE + end + + elseif current_state == FSM_STATE.ACTIVE then + -- Assert the parameter limits. + local ratio = sanitize_ratio(CGA_RATIO:get()) + -- Create the control allocation matrix parameters. + update_dynamic_mixer(ratio) + + else + gcs:send_text(MAV_SEVERITY.CRITICAL, "Unexpected FSM state!") + end + + current_state = next_state +end + +-- Check once on boot if the frame type is suitable for this script. +pcall(inspect_frame_class_mc) +pcall(inspect_frame_class_fw) +gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME .. string.format(" loaded.")) + +-- Wrapper around update() to catch errors. +function protected_wrapper() + local success, err = pcall(fsm_step) + if not success then + gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err) + return protected_wrapper, 1000 + end + if not (current_state == FSM_STATE.FINISHED) then + return protected_wrapper, 1000.0/LOOP_RATE_HZ + end +end + +-- Start running update loop +return protected_wrapper() diff --git a/libraries/AP_Scripting/applets/x-quad-cg-allocation.md b/libraries/AP_Scripting/applets/x-quad-cg-allocation.md new file mode 100644 index 0000000000000..ef876880995e6 --- /dev/null +++ b/libraries/AP_Scripting/applets/x-quad-cg-allocation.md @@ -0,0 +1,65 @@ +# Multicopter CoM compensation + +This script allows for adjusting the control allocation matrix. + +When the Center of Mass (CoM) of an airframe does not coincide with the center +of its thrusters, then there is a lever arm between the thrust vector and the +CoM. This often is the case in VTOL fixed-wing aircraft (quadplanes) where +typically the CoM is more forward than the center of thrust. As a result, the +thrust produces a pitch-down moment. This produces a disturbance in the pitch +control and requires significant wind-up in the pitch integrator. + +To compensate for this issue, this script employs the scriptable control +allocation matrix to request asymmeterical front and back thrust. + +WARNING: This script is applicable only to X-type quadrotors and quadplanes. Do +not use in any other frame configuration! + +# Parameters + +The script adds 1 parameter to control its behaviour. + +## CGA_RATIO + +This is the desired ratio between the front and back thrust. To have the front +motors produce more lift that the rear, increase higher than 1. + +Reasonable extreme values are 2 (front works twice as hard as the rear) and 0.5 +(the inverse case). Given an out-of-bounds parameter value, the script will +revert to the default 1.0. + +# Operation + +## How To Use + +First of all, place this script in the "scripts" directory. + +To tune `CGA_RATIO` on the fly: + + 1. Set `FRAME_CLASS` or `Q_FRAME_CLASS` (for quadplanes) to 17 to enable the + dynamic scriptable mixer. + 2. Enable Lua scripting via the `SCR_ENABLE` parameter. + 3. Reboot. + 4. Fly the vehicle. + 5. Adjust the value of the `CGA_RATIO` parameter. A good indicator of a good + tune is to monitor the telemetry value `PID_TUNE[2].I` (pitch rate controller + integrator) until it reaches zero during a stable hover. + +Once you are happy with the tuning, you can fall back to the static motor +matrix, which consumes no resources from the scripting engine: + + 1. Set `FRAME_CLASS` or `Q_FRAME_CLASS` (for quadplanes) to 15 to enable the + static scriptable mixer. + 2. Ensure Lua scripting is enabled via the `SCR_ENABLE` parameter. + 3. Reboot. + +The aircraft is ready to fly. +Keep in mind that any further changes to `CGA_RATIO` will now require a reboot. + +## How It Works + + 1. The dynamic control allocation matrix is able to change the coefficients + that convert the throttle command to individual PWM commands for every motor. + These coefficients have a default value of 1. + 2. The parameter `CGA_RATIO` is used to alter these coefficients, so that the + front and back thrust commands are not equal. \ No newline at end of file