From 15820ec5f8ee9ad617c25002f97d85973b1a3265 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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
 
 
 --[[