Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: Added script for Quad-X CoM compensation

Browse Source 
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.
This commit is contained in:
George Zogopoulos 2024-06-25 13:43:38 +02:00 committed by Andrew Tridgell
parent 2cd5413b0d
commit 61c910b08b
2 changed files with 348 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

283
libraries/AP_Scripting/applets/x-quad-cg-allocation.lua Normal file
View File

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

65
libraries/AP_Scripting/applets/x-quad-cg-allocation.md Normal file
View File

@ -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.