Skip to content

Commit

Permalink
AP_Scripting: Added script for Quad-X CoM compensation
Browse files Browse the repository at this point in the history
The script uses the Motors_dynamic 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.
  • Loading branch information
Georacer committed Jul 1, 2024
1 parent 580199d commit 8a010a3
Show file tree
Hide file tree
Showing 2 changed files with 288 additions and 0 deletions.
239 changes: 239 additions & 0 deletions libraries/AP_Scripting/applets/x-quad-cg-allocation.lua
Original file line number Diff line number Diff line change
@@ -0,0 +1,239 @@
-- 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_frame_mc_ok = false
local is_frame_fw_ok = false
local last_ratio = 1

-- State machine states.
local FSM_STATE = {
INACTIVE = 0,
INITIALIZE = 1,
ACTIVE = 2,
}
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)
return 1.0 -- Return default.
else
return ratio
end
end

-- Adjust the dynamic motor mixer.
function update_mixer(ratio)
local r1 = 1/(1+ratio)
local r2 = ratio/(1+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.
factors:throttle(0, 2*r2)
factors:throttle(1, 2*r1)
factors:throttle(2, 2*r2)
factors:throttle(3, 2*r1)

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

end

-- Decide if the UA is a Quad X quadplane.
function inspect_frame_class_fw()
local result = false

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 and Q_FRAME_CLASS:get()==17 then
result = true
end
end
is_frame_fw_ok = result
end

-- Decide if the UA is a Quad X multicopter.
function inspect_frame_class_mc()
local result = false

FRAME_CLASS = bind_param("FRAME_CLASS")

if FWVersion:type()==2 then
if FRAME_CLASS:get()==17 then
result = true
end
end
is_frame_mc_ok = result
end

--[[
Activation conditions
--]]
-- Check for script activating conditions here.
-- Check frame types.
function can_start()
result = is_frame_mc_ok or is_frame_fw_ok
return result
end

--[[
Deactivation conditions
--]]
-- Check for script deactivating conditions here.
function must_stop()
return false
end

--[[
State machine
--]]
function fsm_step()
if current_state == FSM_STATE.INACTIVE then
if can_start() then
next_state = FSM_STATE.INITIALIZE
end

elseif current_state == FSM_STATE.INITIALIZE then
next_state = FSM_STATE.ACTIVE

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

if must_stop() then
next_state = FSM_STATE.INACTIVE
end
else
gcs:send_text(MAV_SEVERITY.CRITICAL, "Unexpected FSM state!")
end

current_state = next_state
end

--[[
Main loop function
--]]
function update()
fsm_step()
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(update)
if not success then
gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err)
return protected_wrapper, 1000
end
return protected_wrapper, 1000.0/LOOP_RATE_HZ
end

-- Start running update loop
return protected_wrapper()
49 changes: 49 additions & 0 deletions libraries/AP_Scripting/applets/x-quad-cg-allocation.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
# 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 dynamic 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 it's 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 use
the default 1.0.

# Operation

How To Use
1. Place this script in the "scripts" directory.
2. Set `FRAME_CLASS` or `Q_FRAME_CLASS` (for quadplanes) 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. A good indicator of a good
tune is to monitor the telemetry value `PID_TUNE[1].I` (pitch rate controller
integrator) until it reaches zero during a stable hover.

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.

0 comments on commit 8a010a3

Please sign in to comment.