ardupilot/libraries/AP_Scripting/examples/mission-load.lua

72 lines
2.0 KiB
Lua
Raw Normal View History

-- Example of loading a mission from the SD card using Scripting
-- Would be trivial to select a mission based on scripting params or RC switch
-- luacheck: only 0
local function read_mission(file_name)
-- Open file
file = assert(io.open(file_name), 'Could not open :' .. file_name)
-- check header
assert(string.find(file:read('l'),'QGC WPL 110') == 1, file_name .. ': incorrect format')
-- clear any existing mission
assert(mission:clear(), 'Could not clear current mission')
-- read each line and write to mission
local item = mavlink_mission_item_int_t()
local index = 0
local fail = false
while true and not fail do
local data = {}
local line = file:read()
if not line then
break
end
local ret, _, seq, curr, frame, cmd, p1, p2, p3, p4, x, y, z, autocont = string.find(line, "^(%d+)%s+(%d+)%s+(%d+)%s+(%d+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+(%d+)")
if not ret then
fail = true
break
end
if tonumber(seq) ~= index then
fail = true
break
end
item:seq(tonumber(seq))
item:frame(tonumber(frame))
item:command(tonumber(cmd))
item:param1(tonumber(p1))
item:param2(tonumber(p2))
item:param3(tonumber(p3))
item:param4(tonumber(p4))
if mission:cmd_has_location(tonumber(cmd)) then
item:x(math.floor(tonumber(x)*10^7))
item:y(math.floor(tonumber(y)*10^7))
else
item:x(math.floor(tonumber(x)))
item:y(math.floor(tonumber(y)))
end
item:z(tonumber(z))
if not mission:set_item(index,item) then
mission:clear() -- clear part loaded mission
fail = true
break
end
index = index + 1
end
if fail then
mission:clear() --clear anything already loaded
error(string.format('failed to load mission at seq num %u', index))
end
gcs:send_text(0, string.format("Loaded %u mission items", index))
end
function update()
read_mission('mission1.txt')
return
end
return update, 5000