From 6c785715fa0d212fcc7be6addc63bac769015271 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Nov 2022 07:21:11 +1100 Subject: [PATCH] AP_Scripting: fixed initial ground course in aerobatic tricks needs ground course to account for wind --- .../Aerobatics/Trajectory/plane_aerobatics.lua | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua b/libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua index 0e3611f5f9..71d313ce80 100644 --- a/libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua +++ b/libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua @@ -1127,6 +1127,15 @@ function target_groundspeed() return math.max(ahrs:get_EAS2TAS()*TRIM_ARSPD_CM:get()*0.01, ahrs:get_velocity_NED():length()) end +--[[ + get ground course from AHRS +--]] +function get_ground_course_deg() + local vned = ahrs:get_velocity_NED() + return wrap_180(math.deg(math.atan(vned:y(), vned:x()))) +end + + --args: -- path_f: path function returning position -- t: normalised [0, 1] time @@ -1682,7 +1691,7 @@ function check_auto_mission() -- we've started a new command current_task = nil last_id = id - local initial_yaw_deg = math.deg(ahrs:get_yaw()) + local initial_yaw_deg = get_ground_course_deg() gcs:send_text(0, string.format("Starting %s!", command_table[cmd].name )) -- work out yaw between previous WP and next WP @@ -1788,7 +1797,7 @@ function check_trick() return end gcs:send_text(0, string.format("Trick %u started (%s)", selection, cmd.name)) - local initial_yaw_deg = math.deg(ahrs:get_yaw()) + local initial_yaw_deg = get_ground_course_deg() current_task = PathTask(cmd.fn, cmd.name, nil,