Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: added straight_align path primitive

Browse Source 
allows alignment to position within maneuver frame
This commit is contained in:
Andrew Tridgell 2022-10-31 16:38:53 +11:00
parent 098ed891a2
commit 2224096cff
1 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -584,20 +584,21 @@ function path_composer(_name, _subpaths)
else
-- we are being called with a list function/argument tuples
local args = subpaths[i][2]
cache_sp = subpaths[i][1](args[1], args[2], args[3], args[4])
cache_sp = subpaths[i][1](args[1], args[2], args[3], args[4], start_pos[i], start_orientation[i])
end
return cache_sp
end
for i = 1, num_sub_paths do
local sp = self.subpath(i)
lengths[i] = sp.get_length()
total_length = total_length + lengths[i]
-- accumulate orientation, position and angle
start_orientation[i] = orientation
start_pos[i] = pos
start_angle[i] = angle
local sp = self.subpath(i)
lengths[i] = sp.get_length()
total_length = total_length + lengths[i]
local spos = sp.get_pos(1.0)
orientation:earth_to_body(spos)
pos = pos + spos
@ -723,6 +724,23 @@ function straight_roll(length, num_rolls, arg3, arg4)
})
end
--[[
fly straight until we are distance meters from the composite path
origin in the maneuver frame along the X axis. If we are already
past that position then return immediately
--]]
function straight_align(distance, arg2, arg3, arg4, start_pos, start_orientation)
local d2 = distance - start_pos:x()
local v = makeVector3f(d2, 0, 0)
start_orientation:earth_to_body(v)
local len = math.max(v:x(),0.01)
gcs:send_text(0,string.format("straight_align: %.1f %.1f %.1f", distance, d2, len))
return make_paths("straight_align", {
{ path_straight(len), roll_angle(0) },
})
end
function immelmann_turn(r, roll_rate, arg3, arg4)
local speed = target_groundspeed()
return make_paths("immelmann_turn", {