-
Notifications
You must be signed in to change notification settings - Fork 18k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
AP_Scripting: Added script for Quad-X CoM compensation
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
Showing
2 changed files
with
288 additions
and
0 deletions.
There are no files selected for viewing
239 changes: 239 additions & 0 deletions
239
libraries/AP_Scripting/applets/x-quad-cg-allocation.lua
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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. |