mirror of https://github.com/ArduPilot/ardupilot
142 lines
3.9 KiB
Lua
142 lines
3.9 KiB
Lua
--[[
|
|
create a mission of SCR_USER1 waypoints in a spiral pattern, for
|
|
testing large mission transfer
|
|
|
|
Runs on any change to SCR_USER1
|
|
--]]
|
|
|
|
local SCR_USER1 = Parameter("SCR_USER1") -- number of WPs
|
|
local SCR_USER2 = Parameter("SCR_USER2") -- radius
|
|
local SCR_USER3 = Parameter("SCR_USER3") -- center lat
|
|
local SCR_USER4 = Parameter("SCR_USER4") -- center lon
|
|
local SCR_USER5 = Parameter("SCR_USER5") -- altitude
|
|
local SCR_USER6 = Parameter("SCR_USER6") -- number of WPs for spiral compare
|
|
|
|
local last_SCR_USER1 = SCR_USER1:get()
|
|
local last_SCR_USER6 = SCR_USER6:get()
|
|
|
|
local NAV_WAYPOINT = 16
|
|
local FRAME_GLOBAL = 3
|
|
|
|
local ANGLE_INCREMENT = 5.0
|
|
local MIN_RADIUS = 10.0
|
|
|
|
--[[
|
|
create a waypoint
|
|
--]]
|
|
function create_WP(i, center, radius, angle)
|
|
local item = mavlink_mission_item_int_t()
|
|
local loc = center:copy()
|
|
loc:offset_bearing(radius, angle)
|
|
|
|
item:seq(i)
|
|
item:frame(FRAME_GLOBAL)
|
|
item:command(NAV_WAYPOINT)
|
|
item:param1(0)
|
|
item:param2(0)
|
|
item:param3(0)
|
|
item:param4(0)
|
|
item:x(loc:lat())
|
|
item:y(loc:lng())
|
|
item:z(100+loc:alt()*0.01)
|
|
return item
|
|
end
|
|
|
|
--[[
|
|
create a waypoint based on index
|
|
--]]
|
|
function create_WP_idx(center, idx)
|
|
local radius = SCR_USER2:get()
|
|
if radius < MIN_RADIUS then
|
|
radius = MIN_RADIUS
|
|
end
|
|
return create_WP(idx, center, radius+idx, idx*ANGLE_INCREMENT)
|
|
end
|
|
|
|
--[[
|
|
get center Location
|
|
--]]
|
|
function get_center()
|
|
-- if USER3 and USER4 set then use those, allowing autotest to control exact position
|
|
if SCR_USER3:get() ~= 0 and SCR_USER4:get() ~= 0 then
|
|
local loc = Location()
|
|
loc:lat(math.floor(SCR_USER3:get()*1.0e7))
|
|
loc:lng(math.floor(SCR_USER4:get()*1.0e7))
|
|
loc:alt(math.floor(SCR_USER5:get()*100))
|
|
return loc
|
|
end
|
|
return ahrs:get_location()
|
|
end
|
|
|
|
--[[
|
|
create an N point spiral mission
|
|
--]]
|
|
function create_spiral(N)
|
|
local center = get_center()
|
|
if not center then
|
|
gcs:send_text(0, "Error: Need AHRS location for spiral center")
|
|
return
|
|
end
|
|
mission:clear()
|
|
|
|
gcs:send_text(0, string.format("Creating spiral of size %u", N))
|
|
|
|
for i=0, N-1 do
|
|
local item = create_WP_idx(center, i)
|
|
if not mission:set_item(i, item) then
|
|
gcs:send_text(0, string.format("Failed to create WP %u", i))
|
|
N = i
|
|
break
|
|
end
|
|
end
|
|
gcs:send_text(0, string.format("Created spiral of size %u", N))
|
|
end
|
|
|
|
--[[
|
|
compare current mission with spiral, used in autotest
|
|
--]]
|
|
function compare_spiral(N)
|
|
local center = get_center()
|
|
if not center then
|
|
gcs:send_text(0, "Error: Need AHRS location for spiral center")
|
|
return
|
|
end
|
|
gcs:send_text(0, string.format("Comparing spiral of size %u", N))
|
|
|
|
-- start from index 1 as home can change
|
|
for i=1, N-1 do
|
|
local item1 = create_WP_idx(center, i)
|
|
local item2 = mission:get_item(i)
|
|
if not item2 then
|
|
gcs:send_text(0, string.format("Failed to fetch WP %u", i))
|
|
return
|
|
end
|
|
if item1:x() ~= item2:x() or item1:y() ~= item2:y() or item1:z() ~= item2:z() then
|
|
gcs:send_text(0, string.format("Compared fail %u (%.8f %.8f %.2f) (%.8f %.8f %.2f)", i,
|
|
item1:x()*1.0e-7,
|
|
item1:y()*1.0e-7,
|
|
item1:z(),
|
|
item2:x()*1.0e-7,
|
|
item2:y()*1.0e-7,
|
|
item2:z()))
|
|
return
|
|
end
|
|
end
|
|
gcs:send_text(0, string.format("Compared spiral of size %u OK", N))
|
|
end
|
|
|
|
function update()
|
|
if SCR_USER1:get() ~= last_SCR_USER1 and SCR_USER1:get() > 0 then
|
|
last_SCR_USER1 = SCR_USER1:get();
|
|
create_spiral(last_SCR_USER1);
|
|
end
|
|
if SCR_USER6:get() ~= last_SCR_USER6 and SCR_USER6:get() > 0 then
|
|
last_SCR_USER6 = SCR_USER6:get();
|
|
compare_spiral(last_SCR_USER6);
|
|
end
|
|
return update, 1000
|
|
end
|
|
|
|
gcs:send_text(0, "Loaded spiral mission creator")
|
|
return update, 1000
|