AP_Scripting: simplify path attribute handling

avoid needing a separate accessor function for each attribute
This commit is contained in:
Andrew Tridgell 2022-11-26 08:54:00 +11:00
parent fc137d3bdc
commit 133dcf240b

View File

@ -22,7 +22,6 @@ AEROM_KE_ANG = bind_add_param('KE_ANG', 3, 0)
THR_PIT_FF = bind_add_param('THR_PIT_FF', 4, 80)
SPD_P = bind_add_param('SPD_P', 5, 5)
SPD_I = bind_add_param('SPD_I', 6, 25)
ERR_CORR_TC = bind_add_param('ERR_COR_TC', 7, 3)
ROLL_CORR_TC = bind_add_param('ROL_COR_TC', 8, 0.25)
-- removed 9 and 10
TIME_CORR_P = bind_add_param('TIME_COR_P', 11, 1.0)
@ -55,6 +54,11 @@ SCALING_SPEED = Parameter("SCALING_SPEED")
local GRAVITY_MSS = 9.80665
--[[
list of attributes that can be added to a path element
--]]
local path_attribs = { "roll_ref", "set_orient", "rate_override", "thr_boost", "pos_corr" }
--[[
Aerobatic tricks on a switch support - allows for tricks to be initiated outside AUTO mode
--]]
@ -536,18 +540,11 @@ function PathComponent()
function self.get_roll_correction(t)
return 0
end
function self.get_throttle_boost(t)
return self.thr_boost or false
function self.get_attribute(t, attrib)
return self[attrib]
end
function self.get_rate_override(t)
return self.rate_override
end
function self.get_pos_correction(t)
return self.pos_corr or 1.0
end
--[[ get the extents of the path on x axis. Can be overridden by a
more efficient and accurate path specific function
--]]
@ -837,15 +834,10 @@ function Path(_path_component, _roll_component)
function self.get_final_orientation()
return path_component.get_final_orientation()
end
function self.get_throttle_boost(t)
return self.thr_boost or false
end
function self.get_rate_override(t)
return self.rate_override
end
function self.get_pos_correction(t)
return self.pos_corr or 1.0
function self.get_attribute(t, attrib)
return self[attrib]
end
function self.set_next_extents(extents, start_pos, start_orientation)
path_component.set_next_extents(extents, start_pos, start_orientation)
end
@ -900,9 +892,10 @@ function path_composer(_name, _subpaths)
local args = subpaths[i][2]
cache_sp = subpaths[i][1](args[1], args[2], args[3], args[4], start_pos[i], start_orientation[i])
message = subpaths[i].message
cache_sp.thr_boost = subpaths[i].thr_boost
cache_sp.pos_corr = subpaths[i].pos_corr
cache_sp.rate_override = subpaths[i].rate_override
-- copy over path attributes
for k, v in pairs(path_attribs) do
cache_sp[v] = subpaths[i][v]
end
end
return cache_sp
end
@ -1039,18 +1032,12 @@ function path_composer(_name, _subpaths)
return final_orientation
end
function self.get_throttle_boost(t)
function self.get_attribute(t, attrib)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
return sp.thr_boost or sp.get_throttle_boost(subpath_t)
return sp[attrib] or sp.get_attribute(subpath_t, attrib)
end
function self.get_pos_correction(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
return sp.pos_corr or sp.get_pos_correction(subpath_t)
end
local extents = nil
function self.get_extents_x()
if extents ~= nil then
@ -1060,15 +1047,6 @@ function path_composer(_name, _subpaths)
return extents
end
function self.get_rate_override(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
if sp.rate_override ~= nil then
return sp.rate_override
end
return sp.get_rate_override(subpath_t)
end
--[[
get the time that the next segment starts
--]]
@ -1096,15 +1074,10 @@ function make_paths(name, paths)
else
p[i] = Path(paths[i][1], paths[i][2])
end
if paths[i].roll_ref then
p[i].roll_ref = paths[i].roll_ref
-- copy over path attributes
for k, v in pairs(path_attribs) do
p[i][v] = paths[i][v]
end
if paths[i].set_orient then
p[i].set_orient = paths[i].set_orient
end
p[i].rate_override = paths[i].rate_override
p[i].thr_boost = paths[i].thr_boost
p[i].pos_corr = paths[i].pos_corr
end
return path_composer(name, p)
end
@ -2154,9 +2127,11 @@ function rotate_path(path_f, t, orientation, offset)
local angle = path_f.get_roll(t)
local roll_correction = path_f.get_roll_correction(t)
local speed = path_f.get_speed(t)
local thr_boost = path_f.get_throttle_boost(t)
local pos_corr = path_f.get_pos_correction(t)
local rate_override = path_f.get_rate_override(t)
local attrib = {}
for k, v in pairs(path_attribs) do
attrib[v] = path_f.get_attribute(t, v)
end
local point = quat_earth_to_body(orientation, point)
local scale = AEROM_PATH_SCALE:get()
@ -2171,7 +2146,7 @@ function rotate_path(path_f, t, orientation, offset)
point = quat_body_to_earth((orient * orient), point)
end
return point+offset, math.rad(angle+roll_correction), speed, thr_boost, rate_override, pos_corr
return point+offset, math.rad(angle+roll_correction), speed, attrib
end
--Given vec1, vec2, returns an (rotation axis, angle) tuple that rotates vec1 to be parallel to vec2
@ -2372,13 +2347,13 @@ function do_path()
local p0 = path_var.pos:copy()
local r0 = path_var.roll
local s0 = path_var.speed
local p1, r1, s1, thr_boost, rate_override, pos_corr = rotate_path(path, path_var.path_t + local_n_dt,
path_var.initial_ori, path_var.initial_ef_pos)
local p1, r1, s1, attrib = rotate_path(path, path_var.path_t + local_n_dt,
path_var.initial_ori, path_var.initial_ef_pos)
local current_measured_pos_ef = ahrs_pos_NED:copy()
if rate_override ~= nil then
if not rate_override.update(path, path_var.path_t + local_n_dt, s1) then
if attrib.rate_override ~= nil then
if not attrib.rate_override.update(path, path_var.path_t + local_n_dt, s1) then
-- not done yet
path_var.pos = current_measured_pos_ef
return true
@ -2417,9 +2392,9 @@ function do_path()
--[[
recalculate the current path position and angle based on actual delta time
--]]
local p1, r1, s1, thr_boost = rotate_path(path,
constrain(path_var.path_t + path_t_delta, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
local p1, r1, s1, attrib = rotate_path(path,
constrain(path_var.path_t + path_t_delta, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
local last_path_t = path_var.path_t
path_var.path_t = path_var.path_t + path_t_delta
@ -2448,9 +2423,9 @@ function do_path()
path_var.path_t = path_var.path_t + TIME_CORR_P:get() * path_err_t
-- get the path again with the corrected time
p1, r1, s1, thr_boost = rotate_path(path,
constrain(path_var.path_t, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
p1, r1, s1, attrib = rotate_path(path,
constrain(path_var.path_t, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
-- recalculate the tangent to match the amount we advanced the path time
tangent2_ef = p1 - p0
@ -2485,7 +2460,7 @@ function do_path()
local acc_err_ef = B:scale(ERR_CORR_P:get()) + B_dot:scale(ERR_CORR_D:get())
-- scale by per-maneuver error correction scale factor
acc_err_ef = acc_err_ef:scale(pos_corr)
acc_err_ef = acc_err_ef:scale(attrib.pos_corr or 1.0)
local acc_err_bf = quat_earth_to_body(ahrs_quat, acc_err_ef)
@ -2672,7 +2647,7 @@ function do_path()
local throttle = speed_PI.update(s1, anticipated_pitch_rad)
local thr_min = AEROM_THR_MIN:get()
if thr_boost then
if attrib.thr_boost then
thr_min = math.max(thr_min, AEROM_THR_BOOST:get())
end
throttle = constrain(throttle, thr_min, 100.0)