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 00000000000000..aa5a1bb1a92ca3 --- /dev/null +++ b/libraries/AP_Scripting/applets/x-quad-cg-allocation.lua @@ -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() \ No newline at end of file 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 00000000000000..114bb61f86ebfa --- /dev/null +++ b/libraries/AP_Scripting/applets/x-quad-cg-allocation.md @@ -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. \ No newline at end of file