ardupilot/libraries/AP_Scripting/examples/set_target_posvel_circle.lua

90 lines
3.1 KiB
Lua

-- Commands copter to fly circle trajectory using posvel method in guided mode.
-- The trajectory start from the current location
--
-- CAUTION: This script only works for Copter.
-- This script start when the in GUIDED mode and above 5 meter.
-- 1) arm and takeoff to above 5 m
-- 2) switch to GUIDED mode
-- 3) the vehilce will follow a circle in clockwise direction with increasing speed until ramp_up_time_s time has passed.
-- 4) switch out of and into the GUIDED mode any time to restart the trajectory from the start.
-- Edit these variables
local rad_xy_m = 10.0 -- circle radius in xy plane in m
local target_speed_xy_mps = 5.0 -- maximum target speed in m/s
local ramp_up_time_s = 10.0 -- time to reach target_speed_xy_mps in second
local sampling_time_s = 0.05 -- sampling time of script
-- Fixed variables
local omega_radps = target_speed_xy_mps/rad_xy_m
local copter_guided_mode_num = 4
local theta = 0.0
local time = 0.0
local test_start_location = Vector3f(0.0, 0.0, 0.0)
gcs:send_text(0,"Script started")
gcs:send_text(0,"Trajectory period: " .. tostring(2 * math.rad(180) / omega_radps))
function circle()
local cur_freq = 0
-- increase target speed lineary with time until ramp_up_time_s is reached
if time <= ramp_up_time_s then
cur_freq = omega_radps*(time/ramp_up_time_s)^2
else
cur_freq = omega_radps
end
-- calculate circle reference position and velocity
theta = theta + cur_freq*sampling_time_s
local th_s = math.sin(theta)
local th_c = math.cos(theta)
local pos = Vector3f()
pos:x(rad_xy_m*th_s)
pos:y(-rad_xy_m*(th_c-1))
pos:z(0)
local vel = Vector3f()
vel:x(cur_freq*rad_xy_m*th_c)
vel:y(cur_freq*rad_xy_m*th_s)
vel:z(0)
return pos, vel
end
function update()
if arming:is_armed() and vehicle:get_mode() == copter_guided_mode_num and -test_start_location:z()>=5 then
-- calculate current position and velocity for circle trajectory
local target_pos = Vector3f()
local target_vel = Vector3f()
target_pos, target_vel = circle()
-- advance the time
time = time + sampling_time_s
-- send posvel request
if not vehicle:set_target_posvel_NED(target_pos+test_start_location, target_vel) then
gcs:send_text(0, "Failed to send target posvel at " .. tostring(time) .. " seconds")
end
else
-- calculate test starting location in NED
local cur_loc = ahrs:get_position()
if cur_loc then
test_start_location = cur_loc.get_vector_from_origin_NEU(cur_loc)
if test_start_location then
test_start_location:x(test_start_location:x() * 0.01)
test_start_location:y(test_start_location:y() * 0.01)
test_start_location:z(-test_start_location:z() * 0.01)
end
end
-- reset some variable as soon as we are not in guided mode
time = 0
theta = 0
end
return update, sampling_time_s * 1000
end
return update()