aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Scripting: added barrell_roll

Browse Source 
path OK, but exit orientation is wrong
This commit is contained in:
Andrew Tridgell 2022-11-10 16:50:31 +11:00
parent 3a36a1d405
commit 64e050da60
3 changed files with 148 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Scripting/applets/Aerobatics/FixedWing/README.md
View File

@ -19,33 +19,35 @@ AUTO missions. Once the trick is completed, the mode that was being used at the
track and altitude are reset to the values present when the mode is restored. Tricks in AUTO missions require that they be performed between two waypoints to establish track and altitude are reset to the values present when the mode is restored. Tricks in AUTO missions require that they be performed between two waypoints to establish
the ground track. the ground track.
| ID | Name | Arg1 | Arg2 | Arg3 | Arg4 | Turnaround | | ID | Name | Arg1 | Arg2 | Arg3 | Arg4 | Turnaround |
| -- | ------------------------ | ------ | ---------- | ------- | ---------- | ---------- | | -- | ------------------------ | ------ | ---------- | ------- | ---------- | ---------- |
| 1 | Figure Eight | radius | bank angle | | | No | | 1 | Figure Eight | radius | bank angle | | | No |
| 2 | Loop | radius | bank angle | num loops | | No | | 2 | Loop | radius | bank angle | num loops | | No |
| 3 | Horizontal Rectangle | length | width | radius | bank angle | No | | 3 | Horizontal Rectangle | length | width | radius | bank angle | No |
| 4 | Climbing Circle | radius | height | bank angle | | No | | 4 | Climbing Circle | radius | height | bank angle | | No |
| 5 | vertical Box | length | height | radius | | No | | 5 | vertical Box | length | height | radius | | No |
| 6 | Immelmann (FastRoll) | radius | | | | Yes | | 6 | Immelmann (FastRoll) | radius | | | | Yes |
| 7 | Straight Roll | length | num rolls | | | No | | 7 | Straight Roll | length | num rolls | | | No |
| 8 | Rolling Circle | radius | num rolls | | | No | | 8 | Rolling Circle | radius | num rolls | | | No |
| 9 | Half Cuban Eight | radius | | | | Yes | | 9 | Half Cuban Eight | radius | | | | Yes |
| 10 | Half Reverse Cuban Eight | radius | | | | Yes | | 10 | Half Reverse Cuban Eight | radius | | | | Yes |
| 11 | Cuban Eight | radius | | | | No | | 11 | Cuban Eight | radius | | | | No |
| 12 | Humpty Bump | radius | height | | | Yes | | 12 | Humpty Bump | radius | height | | | Yes |
| 13 | Straight Flight | length | bank angle | | | No | | 13 | Straight Flight | length | bank angle | | | No |
| 14 | Scale Figure Eight | radius | bank angle | | | No | | 14 | Scale Figure Eight | radius | bank angle | | | No |
| 15 | Immelmann Turn | radius | | | | Yes | | 15 | Immelmann Turn | radius | | | | Yes |
| 16 | Split-S | radius | | | | Yes | | 16 | Split-S | radius | | | | Yes |
| 17 | Upline-45 | radius | height gain | | | No | | 17 | Upline-45 | radius | height gain | | | No |
| 18 | Downline-45 | radius | height loss | | | No | | 18 | Downline-45 | radius | height loss | | | No |
| 19 | Stall Turn | radius | height | direction | | Yes | | 19 | Stall Turn | radius | height | direction | | Yes |
| 20 | Procedure Turn | radius | bank angle | step-out | | Yes | | 20 | Procedure Turn | radius | bank angle | step-out | | Yes |
| 21 | Derry Turn | radius | bank angle | | | No | | 21 | Derry Turn | radius | bank angle | | | No |
| 22 | Two Point Roll | length | | | | No | | 22 | Two Point Roll | length | | | | No |
| 23 | Half Climbing Circle | radius | height | bank angle | | Yes | | 23 | Half Climbing Circle | radius | height | bank angle | | Yes |
| 24 | Crossbox Humpty | radius | height | | | Yes | | 24 | Crossbox Humpty | radius | height | | | Yes |
| 25 | Laydown Humpty | radius | height | | | Yes | | 25 | Laydown Humpty | radius | height | | | Yes |
| 25 | Barrell Roll | radius | length | num spirals | | No |
| 26 | Straight Hold | length | bank angle | | | No |
Note: In the script you will find other (specialised) manouvers which do not appear in the Note: In the script you will find other (specialised) manouvers which do not appear in the
'command table'. These tend to be specialised manouvers which may expect an inverted entry or 'command table'. These tend to be specialised manouvers which may expect an inverted entry or

139
libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua
View File

@ -476,6 +476,13 @@ function quat_body_to_earth(quat, v)
return v return v
end end
--[[
copy a quaternion
--]]
function quat_copy(q)
return q:inverse():inverse()
end
--[[ --[[
path component that does a straight horizontal line path component that does a straight horizontal line
--]] --]]
@ -557,16 +564,6 @@ function path_horizontal_arc(_radius, _angle, _height_gain)
function self.get_final_orientation() function self.get_final_orientation()
local q = Quaternion() local q = Quaternion()
q:from_axis_angle(makeVector3f(0,0,1), sgn(radius)*math.rad(angle)) q:from_axis_angle(makeVector3f(0,0,1), sgn(radius)*math.rad(angle))
--[[
we also need to apply the roll correction to the final orientation
--]]
local rc = self.get_roll_correction(1.0)
if rc ~= 0 then
local q2 = Quaternion()
q2:from_axis_angle(makeVector3f(1,0,0), math.rad(-rc))
q = q * q2
q:normalize()
end
return q return q
end end
@ -583,6 +580,42 @@ function path_horizontal_arc(_radius, _angle, _height_gain)
return self return self
end end
--[[
path component that does a cylinder for a barrell roll
--]]
function path_cylinder(_radius, _length, _num_spirals)
local self = PathComponent()
self.name = "path_cylinder"
local radius = _radius
local length = _length
local num_spirals = _num_spirals
local gamma = math.atan((length/num_spirals)/(2*math.pi*radius))
local qrot = Quaternion()
qrot:from_axis_angle(makeVector3f(0,0,1), (0.5*math.pi)-gamma)
function self.get_pos(t)
local t2ang = t * num_spirals * math.pi * 2
local v = makeVector3f(length*t, math.abs(radius)*math.sin(t2ang+math.pi), -radius*(1.0 - math.cos(t2ang)))
return quat_earth_to_body(qrot, v)
end
function self.get_length()
local circumference = 2 * math.pi * math.abs(radius)
local length_per_spiral = length / num_spirals
local helix_length = math.sqrt(length_per_spiral*length_per_spiral + circumference*circumference)
return helix_length * num_spirals
end
--[[
roll correction for the rotation caused by the path
--]]
function self.get_roll_correction(t)
return t*360*math.sin(gamma)*num_spirals
end
return self
end
--[[ --[[
a Path has the methods of both RollComponent and a Path has the methods of both RollComponent and
PathComponent allowing for a complete description of a subpath PathComponent allowing for a complete description of a subpath
@ -593,7 +626,10 @@ function Path(_path_component, _roll_component)
local path_component = _path_component local path_component = _path_component
local roll_component = _roll_component local roll_component = _roll_component
function self.get_roll(t, time_s) function self.get_roll(t, time_s)
return wrap_180(roll_component.get_roll(t, time_s) + path_component.get_roll_correction(t)) return wrap_180(roll_component.get_roll(t, time_s))
end
function self.get_roll_correction(t)
return path_component.get_roll_correction(t)
end end
function self.get_pos(t) function self.get_pos(t)
return path_component.get_pos(t) return path_component.get_pos(t)
@ -628,6 +664,7 @@ function path_composer(_name, _subpaths)
local start_orientation = {} local start_orientation = {}
local start_pos = {} local start_pos = {}
local start_angle = {} local start_angle = {}
local start_roll_correction = {}
local end_speed = {} local end_speed = {}
local start_speed = {} local start_speed = {}
local total_length = 0 local total_length = 0
@ -637,6 +674,7 @@ function path_composer(_name, _subpaths)
local orientation = Quaternion() local orientation = Quaternion()
local pos = makeVector3f(0,0,0) local pos = makeVector3f(0,0,0)
local angle = 0 local angle = 0
local roll_correction = 0
local speed = target_groundspeed() local speed = target_groundspeed()
local highest_i = 0 local highest_i = 0
local cache_i = -1 local cache_i = -1
@ -665,9 +703,10 @@ function path_composer(_name, _subpaths)
for i = 1, num_sub_paths do for i = 1, num_sub_paths do
-- accumulate orientation, position and angle -- accumulate orientation, position and angle
start_orientation[i] = orientation start_orientation[i] = quat_copy(orientation)
start_pos[i] = pos start_pos[i] = pos:copy()
start_angle[i] = angle start_angle[i] = angle
start_roll_correction[i] = roll_correction
local sp = self.subpath(i) local sp = self.subpath(i)
lengths[i] = sp.get_length() lengths[i] = sp.get_length()
@ -676,9 +715,11 @@ function path_composer(_name, _subpaths)
local spos = quat_earth_to_body(orientation, sp.get_pos(1.0)) local spos = quat_earth_to_body(orientation, sp.get_pos(1.0))
pos = pos + spos pos = pos + spos
orientation = orientation * sp.get_final_orientation() orientation = sp.get_final_orientation() * orientation
orientation:normalize() orientation:normalize()
angle = angle + sp.get_roll(1.0, lengths[i]/speed) angle = angle + sp.get_roll(1.0, lengths[i]/speed)
roll_correction = roll_correction + sp.get_roll_correction(1.0)
start_speed[i] = speed start_speed[i] = speed
end_speed[i] = sp.get_speed(1.0) end_speed[i] = sp.get_speed(1.0)
@ -688,15 +729,15 @@ function path_composer(_name, _subpaths)
speed = end_speed[i] speed = end_speed[i]
if sp.roll_ref ~= nil then if sp.roll_ref ~= nil then
q = Quaternion() local q = Quaternion()
q:from_axis_angle(makeVector3f(1,0,0), math.rad(sp.roll_ref)) q:from_axis_angle(makeVector3f(1,0,0), math.rad(sp.roll_ref))
orientation = orientation * q orientation = q * orientation
orientation:normalize() orientation:normalize()
end end
end end
-- get our final orientation, including roll -- get our final orientation, including roll
local final_orientation = orientation local final_orientation = quat_copy(orientation)
local q = Quaternion() local q = Quaternion()
q:from_axis_angle(makeVector3f(1,0,0), math.rad(wrap_180(angle))) q:from_axis_angle(makeVector3f(1,0,0), math.rad(wrap_180(angle)))
final_orientation = q * final_orientation final_orientation = q * final_orientation
@ -753,6 +794,12 @@ function path_composer(_name, _subpaths)
return angle + start_angle[i] return angle + start_angle[i]
end end
function self.get_roll_correction(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
return sp.get_roll_correction(subpath_t) + start_roll_correction[i]
end
-- return speed for the composed path at time t -- return speed for the composed path at time t
function self.get_speed(t) function self.get_speed(t)
local subpath_t, i = self.get_subpath_t(t) local subpath_t, i = self.get_subpath_t(t)
@ -783,7 +830,11 @@ end
function make_paths(name, paths) function make_paths(name, paths)
local p = {} local p = {}
for i = 1, #paths do for i = 1, #paths do
p[i] = Path(paths[i][1], paths[i][2]) if paths[i][2] == nil then
p[i] = paths[i][1]
else
p[i] = Path(paths[i][1], paths[i][2])
end
if paths[i].roll_ref then if paths[i].roll_ref then
p[i].roll_ref = paths[i].roll_ref p[i].roll_ref = paths[i].roll_ref
end end
@ -808,6 +859,12 @@ function half_climbing_circle(radius, height, bank_angle, arg4)
}) })
end end
function partial_circle(radius, bank_angle, angle)
return make_paths("partial_circle", {
{ path_horizontal_arc(radius, angle, 0), roll_angle_entry_exit(bank_angle) },
})
end
function loop(radius, bank_angle, num_loops, arg4) function loop(radius, bank_angle, num_loops, arg4)
if not num_loops or num_loops <= 0 then if not num_loops or num_loops <= 0 then
num_loops = 1 num_loops = 1
@ -949,12 +1006,37 @@ function rolling_circle(radius, num_rolls, arg3, arg4)
}) })
end end
function cylinder(radius, length, num_spirals, arg4)
return make_paths("cylinder", {
{ path_cylinder(radius, length, num_spirals), roll_angle(0), thr_boost=true },
})
end
function barrell_roll(radius, length, num_spirals, arg4)
local gamma_deg = math.deg(math.atan((length/num_spirals)/(2*math.pi*radius)))
local speed = target_groundspeed()
local bank = math.deg(math.atan((speed*speed) / (radius * GRAVITY_MSS)))
return make_paths("barrell_roll", {
{ path_horizontal_arc(-radius, 90-gamma_deg, 0), roll_angle_entry_exit(-bank) },
{ path_cylinder(radius, length, num_spirals), roll_angle(0) },
{ path_horizontal_arc(radius, 90-gamma_deg, 0), roll_angle_entry_exit(bank) },
})
end
function straight_flight(length, bank_angle, arg3, arg4) function straight_flight(length, bank_angle, arg3, arg4)
return make_paths("straight_flight", { return make_paths("straight_flight", {
{ path_straight(length), roll_angle_entry_exit(bank_angle) }, { path_straight(length), roll_angle_entry_exit(bank_angle) },
}) })
end end
function straight_hold(length, bank_angle, arg3, arg4)
return make_paths("straight_hold", {
{ path_straight(length), roll_angle_entry(bank_angle) },
})
end
function scale_figure_eight(r, bank_angle, arg3, arg4) function scale_figure_eight(r, bank_angle, arg3, arg4)
local rabs = math.abs(r) local rabs = math.abs(r)
return make_paths("scale_figure_eight", { return make_paths("scale_figure_eight", {
@ -1638,10 +1720,11 @@ function rotate_path(path_f, t, orientation, offset)
local t = constrain(t, 0, 1) local t = constrain(t, 0, 1)
local point = path_f.get_pos(t) local point = path_f.get_pos(t)
local angle = path_f.get_roll(t) local angle = path_f.get_roll(t)
local roll_correction = path_f.get_roll_correction(t)
local speed = path_f.get_speed(t) local speed = path_f.get_speed(t)
local thr_boost = path_f.get_throttle_boost(t) local thr_boost = path_f.get_throttle_boost(t)
local point = quat_earth_to_body(orientation, point) local point = quat_earth_to_body(orientation, point)
return point+offset, math.rad(angle), speed, thr_boost return point+offset, math.rad(angle+roll_correction), speed, thr_boost
end end
--Given vec1, vec2, returns an (rotation axis, angle) tuple that rotates vec1 to be parallel to vec2 --Given vec1, vec2, returns an (rotation axis, angle) tuple that rotates vec1 to be parallel to vec2
@ -2164,6 +2247,9 @@ command_table[22]= PathFunction(two_point_roll, "Two Point Roll")
command_table[23]= PathFunction(half_climbing_circle, "Half Climbing Circle") command_table[23]= PathFunction(half_climbing_circle, "Half Climbing Circle")
command_table[24]= PathFunction(crossbox_humpty, "Crossbox Humpty") command_table[24]= PathFunction(crossbox_humpty, "Crossbox Humpty")
command_table[25]= PathFunction(laydown_humpty, "Laydown Humpty") command_table[25]= PathFunction(laydown_humpty, "Laydown Humpty")
command_table[26] = PathFunction(barrell_roll, "Barrell Roll")
command_table[27]= PathFunction(straight_flight, "Straight Hold")
command_table[28] = PathFunction(partial_circle, "Partial Circle")
command_table[200] = PathFunction(test_all_paths, "Test Suite") command_table[200] = PathFunction(test_all_paths, "Test Suite")
command_table[201] = PathFunction(nz_clubman, "NZ Clubman") command_table[201] = PathFunction(nz_clubman, "NZ Clubman")
command_table[202] = PathFunction(f3a_p23_l_r, "FAI F3A P23 L to R") command_table[202] = PathFunction(f3a_p23_l_r, "FAI F3A P23 L to R")
@ -2203,6 +2289,9 @@ load_table["crossbox_humpty"] = crossbox_humpty
load_table["laydown_humpty"] = laydown_humpty load_table["laydown_humpty"] = laydown_humpty
load_table["straight_align"] = straight_align load_table["straight_align"] = straight_align
load_table["figure_eight"] = figure_eight load_table["figure_eight"] = figure_eight
load_table["barrell_roll"] = barrell_roll
load_table["straight_hold"] = straight_hold
load_table["partial_circle"] = partial_circle
--[[ --[[
@ -2250,12 +2339,12 @@ function load_trick(id)
local f = load_table[cmd] local f = load_table[cmd]
if f == nil then if f == nil then
gcs:send_text(0,string.format("Unknown command '%s' in %s", cmd, fname)) gcs:send_text(0,string.format("Unknown command '%s' in %s", cmd, fname))
return else
end paths[#paths+1] = { f, { arg1, arg2, arg3, arg4 }}
paths[#paths+1] = { f, { arg1, arg2, arg3, arg4 }} if message ~= nil then
if message ~= nil then paths[#paths].message = message
paths[#paths].message = message message = nil
message = nil end
end end
end end
end end

13
libraries/AP_Scripting/applets/Aerobatics/FixedWing/trick72.txt
View File

@ -32,7 +32,7 @@ split_s 30
straight_align 0 straight_align 0
message: RollingCircle message: RollingCircle
rolling_circle -50 3 rolling_circle -50 2
straight_align -50 straight_align -50
message: HumptyBump message: HumptyBump
@ -43,14 +43,11 @@ message: HalfCubanEight
half_cuban_eight 25 half_cuban_eight 25
straight_align 75 straight_align 75
message: Upline45 message: BarrellRoll
upline_45 30 50 barrell_roll 30 100 2
message: Downline45 message: ProcedureTurn
downline_45 30 50 procedure_turn 30 45 30
message: HalfReverseCubanEight
half_reverse_cuban_eight 25
# get back to the start point # get back to the start point
straight_align 0 straight_align 0