From 15820ec5f8ee9ad617c25002f97d85973b1a3265 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 20 Nov 2022 15:50:10 +1100 Subject: [PATCH] AP_Scripting: added set_orient and funny_loop this allows much tighter control over path orientation during a sequence --- .../Aerobatics/FixedWing/plane_aerobatics.lua | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index 5b4f03c41e..163f5dc303 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -513,6 +513,15 @@ function PathComponent() return self end +--[[ + return a quaternion for a roll, pitch, yaw (321 euler sequence) attitude +--]] +function qorient(roll_deg, pitch_deg, yaw_deg) + local q = Quaternion() + q:from_euler(math.rad(roll_deg), math.rad(pitch_deg), math.rad(yaw_deg)) + return q +end + --[[ rotate a vector by a quaternion --]] @@ -783,6 +792,11 @@ function path_composer(_name, _subpaths) end speed = end_speed[i] + if sp.set_orient ~= nil then + -- override orientation at this point in the sequence + orientation = sp.set_orient + end + if sp.roll_ref ~= nil then local q = Quaternion() q:from_axis_angle(makeVector3f(1,0,0), math.rad(sp.roll_ref)) @@ -893,6 +907,9 @@ function make_paths(name, paths) if paths[i].roll_ref then p[i].roll_ref = paths[i].roll_ref end + if paths[i].set_orient then + p[i].set_orient = paths[i].set_orient + end p[i].thr_boost = paths[i].thr_boost end return path_composer(name, p) @@ -1543,6 +1560,16 @@ function fai_f3a_box_l_r() }) end +function funny_loop(radius, arg2, arg3, arg4) + return make_paths("funny_loop", { + { path_horizontal_arc(radius, 90), roll_angle(180) }, + { path_vertical_arc(radius, 90), roll_angle(0), set_orient=qorient(0,90,90)}, + { path_horizontal_arc(radius, 180), roll_angle(360), set_orient=qorient(0,-90,-90) }, + { path_vertical_arc(radius, 90), roll_angle(0), set_orient=qorient(0,0,-90) }, + { path_horizontal_arc(radius, 90), roll_angle(-180) }, + }) +end + --[[ NZ clubman schedule --]] @@ -2433,6 +2460,7 @@ load_table["p23_14"] = p23_14 load_table["p23_15"] = p23_15 load_table["p23_16"] = p23_16 load_table["p23_17"] = p23_17 +load_table["funny_loop"] = funny_loop --[[