Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: fixed initial ground course in aerobatic tricks

Browse Source 
needs ground course to account for wind
This commit is contained in:
Andrew Tridgell 2022-11-01 07:21:11 +11:00
parent 2224096cff
commit 6c785715fa
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Scripting/examples/Aerobatics/Trajectory/plane_aerobatics.lua
View File

@ -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,