AP_Scripting: fixed roll correction at end of partial climbing circle

This commit is contained in:
Andrew Tridgell 2022-12-14 08:24:19 +11:00
parent afc1a38b73
commit ca3be1d0c7

View File

@ -765,8 +765,8 @@ function _path_horizontal_arc:get_roll_correction(t)
if self.height_gain == 0 then
return 0
end
local gamma=math.atan(self.height_gain*(self.angle/360)/(2*math.pi*self.radius))
return -t*360*math.sin(gamma)
local gamma=math.atan(self.height_gain*(360/self.angle)/(2*math.pi*self.radius))
return -t*self.angle*math.sin(gamma)
end
function path_horizontal_arc(radius, angle, height_gain)