mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: convert remaining paths to composite functions
This commit is contained in:
parent
7541eed80c
commit
4e154b17a2
|
@ -11,27 +11,27 @@ The following table gives the available manoeuvres. Each manoeuvre has
|
||||||
an ID number which is used in the AUTO mission or in the TRIKn_ID
|
an ID number which is used in the AUTO mission or in the TRIKn_ID
|
||||||
parameters (described below).
|
parameters (described below).
|
||||||
|
|
||||||
| 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 | | | No |
|
| 4 | Climbing Circle | radius | height | bank angle | | No |
|
||||||
| 5 | vertical Box | length | height | radius | | No |
|
| 5 | vertical Box | length | height | radius | | No |
|
||||||
| 6 | Banked Circle | radius | bank angle | height | | No |
|
| 6 | Banked Circle | radius | bank angle | height | | No |
|
||||||
| 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 | roll rate | | | Yes |
|
| 15 | Immelmann Turn | radius | roll rate | | | Yes |
|
||||||
| 16 | Split-S | radius | roll rate | | | Yes |
|
| 16 | Split-S | radius | roll rate | | | 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 |
|
||||||
|
|
||||||
The "Turnaround" column indicates if the manoeuvre results in a course
|
The "Turnaround" column indicates if the manoeuvre results in a course
|
||||||
reversal, which impacts how it is used in AUTO missions.
|
reversal, which impacts how it is used in AUTO missions.
|
||||||
|
|
|
@ -263,30 +263,23 @@ function resolve_jump(i)
|
||||||
return i
|
return i
|
||||||
end
|
end
|
||||||
|
|
||||||
--------Trajectory definitions---------------------
|
--[[
|
||||||
|
Wrapper to construct a Vector3f{x, y, z} from (x, y, z)
|
||||||
function climbing_circle(t, radius, height, arg3, arg4)
|
--]]
|
||||||
local angle = t*math.pi*2
|
function makeVector3f(x, y, z)
|
||||||
local vec = makeVector3f(radius*math.sin(angle), radius*(1.0-math.cos(angle)), -math.sin(0.5*angle)*height)
|
local vec = Vector3f()
|
||||||
return vec, 0.0
|
vec:x(x)
|
||||||
|
vec:y(y)
|
||||||
|
vec:z(z)
|
||||||
|
return vec
|
||||||
end
|
end
|
||||||
|
|
||||||
function loop(t, radius, bank_angle, arg3, arg4)
|
|
||||||
local num_loops = math.abs(arg3)
|
|
||||||
if(arg3 <= 0.0) then
|
|
||||||
num_loops = 1
|
|
||||||
end
|
|
||||||
|
|
||||||
t = num_loops*t*math.pi*2
|
|
||||||
local vec = makeVector3f(math.sin(t), 0.0, -1.0 + math.cos(t))
|
|
||||||
return vec:scale(radius), math.rad(bank_angle)
|
|
||||||
end
|
|
||||||
|
|
||||||
function straight_roll(t, length, num_rolls, arg3, arg4)
|
--[[
|
||||||
|
trajectory building blocks. We have two types of building blocks,
|
||||||
local vec = makeVector3f(t*length, 0.0, 0.0)
|
roll blocks and path blocks. These are combined to give composite paths
|
||||||
return vec, t*num_rolls*2*math.pi
|
--]]
|
||||||
end
|
|
||||||
|
|
||||||
--[[
|
--[[
|
||||||
roll component that goes through a fixed total angle at a fixed roll rate
|
roll component that goes through a fixed total angle at a fixed roll rate
|
||||||
|
@ -412,18 +405,8 @@ function path_horizontal_arc(_radius, _angle, _height_gain)
|
||||||
return self
|
return self
|
||||||
end
|
end
|
||||||
|
|
||||||
--Wrapper to construct a Vector3f{x, y, z} from (x, y, z)
|
|
||||||
function makeVector3f(x, y, z)
|
|
||||||
local vec = Vector3f()
|
|
||||||
vec:x(x)
|
|
||||||
vec:y(y)
|
|
||||||
vec:z(z)
|
|
||||||
return vec
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
--[[
|
--[[
|
||||||
componse multuple sub-paths together
|
componse multiple sub-paths together to create a full trajectory
|
||||||
--]]
|
--]]
|
||||||
function path_composer(_subpaths)
|
function path_composer(_subpaths)
|
||||||
local self = {}
|
local self = {}
|
||||||
|
@ -494,6 +477,40 @@ function path_composer(_subpaths)
|
||||||
return self
|
return self
|
||||||
end
|
end
|
||||||
|
|
||||||
|
--[[
|
||||||
|
composed trajectories, does as individual aerobatic maneuvers
|
||||||
|
--]]
|
||||||
|
|
||||||
|
function climbing_circle(t, radius, height, bank_angle, arg4)
|
||||||
|
if t == 0 then
|
||||||
|
local speed = path_var.target_speed
|
||||||
|
path_var.composer = path_composer({
|
||||||
|
{ path_horizontal_arc(radius, 360, height), roll_angle_entry_exit(bank_angle) },
|
||||||
|
})
|
||||||
|
end
|
||||||
|
return path_var.composer.run(t)
|
||||||
|
end
|
||||||
|
|
||||||
|
function loop(t, radius, bank_angle, arg3, arg4)
|
||||||
|
if t == 0 then
|
||||||
|
local speed = path_var.target_speed
|
||||||
|
path_var.composer = path_composer({
|
||||||
|
{ path_vertical_arc(radius, 360), roll_angle_entry_exit(bank_angle) },
|
||||||
|
})
|
||||||
|
end
|
||||||
|
return path_var.composer.run(t)
|
||||||
|
end
|
||||||
|
|
||||||
|
function straight_roll(t, length, num_rolls, arg3, arg4)
|
||||||
|
if t == 0 then
|
||||||
|
local speed = path_var.target_speed
|
||||||
|
path_var.composer = path_composer({
|
||||||
|
{ path_straight(length), roll_angle(num_rolls*360) },
|
||||||
|
})
|
||||||
|
end
|
||||||
|
return path_var.composer.run(t)
|
||||||
|
end
|
||||||
|
|
||||||
function immelmann_turn(t, r, roll_rate, arg3, arg4)
|
function immelmann_turn(t, r, roll_rate, arg3, arg4)
|
||||||
if t == 0 then
|
if t == 0 then
|
||||||
local speed = path_var.target_speed
|
local speed = path_var.target_speed
|
||||||
|
@ -780,26 +797,6 @@ function rotate_path(path_f, t, arg1, arg2, arg3, arg4, orientation, offset)
|
||||||
return point+offset, angle, speed
|
return point+offset, angle, speed
|
||||||
end
|
end
|
||||||
|
|
||||||
--args:
|
|
||||||
-- dt: sample time
|
|
||||||
-- cutoff_freq: cutoff frequency for low pass filter, in Hz
|
|
||||||
--returns: alpha value required to implement LP filter
|
|
||||||
function calc_lowpass_alpha_dt(dt, cutoff_freq)
|
|
||||||
if dt <= 0.0 or cutoff_freq <= 0.0 then
|
|
||||||
return 1.0
|
|
||||||
end
|
|
||||||
|
|
||||||
local rc = 1.0/(2.0*3.14159265*cutoff_freq)
|
|
||||||
local drc = dt/(dt+rc)
|
|
||||||
if drc < 0.0 then
|
|
||||||
return 0.0
|
|
||||||
end
|
|
||||||
if drc > 1.0 then
|
|
||||||
return 1.0
|
|
||||||
end
|
|
||||||
return drc
|
|
||||||
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
|
||||||
--If vec1 and vec2 are already parallel, returns a zero vector and zero angle
|
--If vec1 and vec2 are already parallel, returns a zero vector and zero angle
|
||||||
--Note that the rotation will not be unique.
|
--Note that the rotation will not be unique.
|
||||||
|
@ -842,12 +839,12 @@ function vectors_to_angular_rate_w_roll(vector1, vector2, time_constant, roll)
|
||||||
return axis:scale(angular_velocity)
|
return axis:scale(angular_velocity)
|
||||||
end
|
end
|
||||||
|
|
||||||
|
-- convert a quaternion to axis angle form
|
||||||
function to_axis_and_angle(quat)
|
function to_axis_and_angle(quat)
|
||||||
|
|
||||||
local axis_angle = Vector3f()
|
local axis_angle = Vector3f()
|
||||||
quat:to_axis_angle(axis_angle)
|
quat:to_axis_angle(axis_angle)
|
||||||
angle = axis_angle:length()
|
angle = axis_angle:length()
|
||||||
if(angle < 0.00001) then
|
if angle < 0.00001 then
|
||||||
return makeVector3f(1.0, 0.0, 0.0), 0.0
|
return makeVector3f(1.0, 0.0, 0.0), 0.0
|
||||||
end
|
end
|
||||||
return axis_angle:scale(1.0/angle), angle
|
return axis_angle:scale(1.0/angle), angle
|
||||||
|
|
Loading…
Reference in New Issue