Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: convert more paths to use composer

This commit is contained in:
Andrew Tridgell 2022-10-27 15:19:30 +11:00
parent b1e123df9e
commit 8bc847fcf1
1 changed files with 65 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

226
libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua
View File

@ -271,57 +271,6 @@ function climbing_circle(t, radius, height, arg3, arg4)
return vec, 0.0 return vec, 0.0
end end
function figure_eight(t, r, bank_angle, arg3, arg4)
local r_sign = sgn(r)
assert(math.abs(r_sign) > 0.1)
local r = math.abs(r)
local T = 3.0*math.pi*r + r*math.sqrt(2) + 2*r
local rsqr2 = r*math.sqrt(2)
local pos
local roll
if (t < rsqr2/T) then
pos = makeVector3f(T*t, 0.0, 0.0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0)/T) then
pos = makeVector3f(r*math.cos(T*t/r - math.sqrt(2) - math.pi/2)+rsqr2, (r + r*math.sin(T*t/r - math.sqrt(2) - math.pi/2)), 0)
roll = math.rad(bank_angle)
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r/4)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2))), 0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + 3*r/4)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2))), 0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2))), 0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0)/T) then
pos = makeVector3f(r*math.cos(-T*t/r +5.0*math.pi/4.0 + math.sqrt(2) + 1 - math.pi/4) - r*math.sqrt(2.0), (r + r*math.sin(-T*t/r +5.0*math.pi/4.0 + math.sqrt(2) + 1 - math.pi/4)), 0)
roll = -math.rad(bank_angle)
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + r/4.0)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2))), 0)
roll = 0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + 3*r/4.0)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2))), 0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + r)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), (r +r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2))), 0)
roll = 0.0
else
pos = makeVector3f(r*math.cos(T*t/r - (6*math.pi/4.0 + math.sqrt(2) +2 ))+rsqr2, (r + r*math.sin(T*t/r - (6*math.pi/4.0 + math.sqrt(2) +2 ))), 0)
roll = math.rad(bank_angle)
end
if r_sign == -1 then
pos:y(-pos:y())
end
return pos, roll
end
function loop(t, radius, bank_angle, arg3, arg4) function loop(t, radius, bank_angle, arg3, arg4)
local num_loops = math.abs(arg3) local num_loops = math.abs(arg3)
if(arg3 <= 0.0) then if(arg3 <= 0.0) then
@ -339,114 +288,6 @@ function straight_roll(t, length, num_rolls, arg3, arg4)
return vec, t*num_rolls*2*math.pi return vec, t*num_rolls*2*math.pi
end end
function half_cuban_eight(t, r, unused, arg3, arg4)
local T = 3.0*math.pi*r/2.0 + 2*r*math.sqrt(2) + r
local trsqr2 = 2*r*math.sqrt(2)
local pos
local roll
if (t < trsqr2/T) then
pos = makeVector3f(T*t, 0.0, 0.0)
roll = 0.0
elseif (t < (trsqr2 + 5.0*math.pi*r/4.0)/T) then
pos = makeVector3f(r*math.cos(T*t/r - 2*math.sqrt(2) - math.pi/2)+trsqr2, 0, -r - r*math.sin(T*t/r - 2*math.sqrt(2) - math.pi/2))
roll = 0.0
elseif (t < (trsqr2 + 5.0*math.pi*r/4.0 + r/4)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = 0.0
elseif (t < (trsqr2 + 5.0*math.pi*r/4.0 + 3*r/4)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = (t - (trsqr2 + 5.0*math.pi*r/4.0 + r/4)/T)*2*math.pi*T/(r)
elseif (t < (trsqr2 + 5.0*math.pi*r/4.0 + r)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = math.pi
else
pos = makeVector3f(r*math.cos(-T*t/r +5.0*math.pi/4.0 + 2*math.sqrt(2) + 1 - math.pi/4), 0, -r -r*math.sin(-T*t/r +5.0*math.pi/4.0 + 2*math.sqrt(2) + 1 - math.pi/4))
roll = math.pi
--roll = 0
end
return pos, roll
end
function cuban_eight(t, r, unused, arg3, arg4)
local T = 3.0*math.pi*r + r*math.sqrt(2) + 2*r
local rsqr2 = r*math.sqrt(2)
local pos
local roll
if (t < rsqr2/T) then
pos = makeVector3f(T*t, 0.0, 0.0)
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0)/T) then
pos = makeVector3f(r*math.cos(T*t/r - math.sqrt(2) - math.pi/2)+rsqr2, 0, -r - r*math.sin(T*t/r - math.sqrt(2) - math.pi/2))
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r/4)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)))
roll = 0.0
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + 3*r/4)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)))
roll = (t - (rsqr2 + 5.0*math.pi*r/4.0 + r/4)/T)*2*math.pi*T/(r)
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r)/T) then
pos = makeVector3f(r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2))*(-r*math.sqrt(2)))
roll = math.pi
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0)/T) then
pos = makeVector3f(r*math.cos(-T*t/r +5.0*math.pi/4.0 + math.sqrt(2) + 1 - math.pi/4) - r*math.sqrt(2.0), 0, -r - r*math.sin(-T*t/r +5.0*math.pi/4.0 + math.sqrt(2) + 1 - math.pi/4))
roll = math.pi
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + r/4.0)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2)))
roll = math.pi
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + 3*r/4.0)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2)))
roll = math.pi +(t - (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + r/4.0)/T)*2 *math.pi*T/(r)
elseif (t < (rsqr2 + 5.0*math.pi*r/4.0 + r + 3*math.pi*r/2.0 + r)/T) then
pos = makeVector3f(-r/math.sqrt(2) + (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2)*(r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*t/r - 5*math.pi/4 - math.sqrt(2) - 1 - 3*math.pi/2.0 )*(-r*math.sqrt(2)))
roll = 0.0
else
pos = makeVector3f(r*math.cos(T*t/r - (6*math.pi/4.0 + math.sqrt(2) +2 ))+rsqr2, 0, -r - r*math.sin(T*t/r - (6*math.pi/4.0 + math.sqrt(2) +2 )))
roll = 0.0
end
return pos, roll
end
function half_reverse_cuban_eight(t, r, arg2, arg3, arg4)
local T = 3.0*math.pi*r/2.0 + 2*r*math.sqrt(2) + r
local trsqr2 = 2*r*math.sqrt(2)
local pos
local roll
if(t < (math.pi*r/4)/T) then
pos = makeVector3f(r*math.cos(-T*(1-t)/r +5.0*math.pi/4.0 + 2*math.sqrt(2) + 1 - math.pi/4), 0, -r -r*math.sin(-T*(1-t)/r +5.0*math.pi/4.0 + 2*math.sqrt(2) + 1 - math.pi/4))
roll = 0
elseif (t < (math.pi*r/4 + r/4)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = 0
elseif (t < (math.pi*r/4 + 3*r/4)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = (t - (math.pi*r/4 + r/4)/T)*2*math.pi*T/(r)
elseif (t < (math.pi*r/4 + r)/T) then
pos = makeVector3f(3*r/math.sqrt(2) + (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)), 0, -r -r/math.sqrt(2) - (T*(1-t)/r - 5*math.pi/4 - 2*math.sqrt(2))*(-r*math.sqrt(2)))
roll = math.pi
elseif (t < (3*math.pi*r/2 + r) /T) then
pos = makeVector3f(r*math.cos(T*(1-t)/r - 2*math.sqrt(2) - math.pi/2)+trsqr2, 0, -r - r*math.sin(T*(1-t)/r - 2*math.sqrt(2) - math.pi/2))
roll = math.pi
else
pos = makeVector3f(T*(1-t), 0.0, 0.0)
roll = math.pi
end
return pos, roll
end
--todo: change y coordinate to z for vertical box --todo: change y coordinate to z for vertical box
--function aerobatic_box(t, l, w, r): --function aerobatic_box(t, l, w, r):
function horizontal_rectangle(t, total_length, total_width, r, bank_angle) function horizontal_rectangle(t, total_length, total_width, r, bank_angle)
@ -560,7 +401,7 @@ function path_vertical_arc(_radius, _angle)
return makeVector3f(math.abs(radius)*math.sin(t2ang), 0, -radius*(1.0 - math.cos(t2ang))) return makeVector3f(math.abs(radius)*math.sin(t2ang), 0, -radius*(1.0 - math.cos(t2ang)))
end end
function self.get_length() function self.get_length()
return math.abs(radius) * 2 * math.pi * angle / 360.0 return math.abs(radius) * 2 * math.pi * math.abs(angle) / 360.0
end end
function self.get_final_orientation() function self.get_final_orientation()
local q = Quaternion() local q = Quaternion()
@ -691,7 +532,6 @@ function path_composer(_subpaths)
local speed = start_speed[i] + subpath_t * (end_speed[i] - start_speed[i]) local speed = start_speed[i] + subpath_t * (end_speed[i] - start_speed[i])
pos = subpaths[i][1].get_pos(subpath_t) pos = subpaths[i][1].get_pos(subpath_t)
angle = subpaths[i][2].get_roll(subpath_t, lengths[i]/speed) angle = subpaths[i][2].get_roll(subpath_t, lengths[i]/speed)
logger.write("SUBA", "i,t,st,time_s,ang", "Bffff", i, t, subpath_t, lengths[i]/speed, angle)
start_orientation[i]:earth_to_body(pos) start_orientation[i]:earth_to_body(pos)
return pos + start_pos[i], math.rad(angle + start_angle[i]), speed return pos + start_pos[i], math.rad(angle + start_angle[i]), speed
end end
@ -818,6 +658,21 @@ function scale_figure_eight(t, r, bank_angle, arg3, arg4)
return path_var.composer.run(t) return path_var.composer.run(t)
end end
function figure_eight(t, r, bank_angle, arg3, arg4)
if t == 0 then
local rabs = math.abs(r)
path_var.composer = path_composer({
{ path_straight(rabs*math.sqrt(2)), roll_angle(0) },
{ path_horizontal_arc(r, 225), roll_angle_entry_exit(bank_angle) },
{ path_straight(2*rabs), roll_angle(0) },
{ path_horizontal_arc(-r, 270), roll_angle_entry_exit(-bank_angle) },
{ path_straight(2*rabs), roll_angle(0) },
{ path_horizontal_arc(r, 45), roll_angle_entry_exit(bank_angle) },
})
end
return path_var.composer.run(t)
end
--[[ --[[
stall turn is not really correct, as we don't fully stall. Needs to be stall turn is not really correct, as we don't fully stall. Needs to be
@ -838,6 +693,54 @@ function stall_turn(t, radius, height, direction, min_speed)
return path_var.composer.run(t) return path_var.composer.run(t)
end end
function half_cuban_eight(t, r, arg2, arg3, arg4)
if t == 0 then
local rabs = math.abs(r)
path_var.composer = path_composer({
{ path_straight(2*rabs*math.sqrt(2)), roll_angle(0) },
{ path_vertical_arc(r, 225), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(180) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_vertical_arc(-r, 45), roll_angle(0) },
})
end
return path_var.composer.run(t)
end
function cuban_eight(t, r, arg2, arg3, arg4)
if t == 0 then
local rabs = math.abs(r)
path_var.composer = path_composer({
{ path_straight(rabs*math.sqrt(2)), roll_angle(0) },
{ path_vertical_arc(r, 225), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(180) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_vertical_arc(-r, 270), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(180) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_vertical_arc(r, 45), roll_angle(0) },
})
end
return path_var.composer.run(t)
end
function half_reverse_cuban_eight(t, r, arg2, arg3, arg4)
if t == 0 then
local rabs = math.abs(r)
path_var.composer = path_composer({
{ path_vertical_arc(r, 45), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_straight(2*rabs/3), roll_angle(180) },
{ path_straight(2*rabs/3), roll_angle(0) },
{ path_vertical_arc(-r, 225), roll_angle(0) },
})
end
return path_var.composer.run(t)
end
--------------------------------------------------- ---------------------------------------------------
--[[ --[[
@ -1271,6 +1174,7 @@ command_table[16]= PathFunction(split_s, "Split-S")
command_table[17]= PathFunction(upline_45, "Upline-45") command_table[17]= PathFunction(upline_45, "Upline-45")
command_table[18]= PathFunction(downline_45, "Downline-45") command_table[18]= PathFunction(downline_45, "Downline-45")
command_table[19]= PathFunction(stall_turn, "Stall Turn") command_table[19]= PathFunction(stall_turn, "Stall Turn")
-- command_table[100] = PathFunction(clubman_schedule, "Clubman Schedule")
-- get a location structure from a waypoint number -- get a location structure from a waypoint number
function get_location(i) function get_location(i)