AP_Scripting: Added script for Quad-X CoM compensation

The script uses the scripting matrix to produce non-equal
front and back thrust, compensating for the lever arm between the center
of thrust and the center of mass.

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

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.