You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
283 lines
9.5 KiB
283 lines
9.5 KiB
-- mission editing demo lua script. |
|
-- by Buzz 2020 |
|
current_pos = nil |
|
home = 0 |
|
a = {} |
|
demostage = 0 |
|
eventcounter = 0 |
|
|
|
function update () -- periodic function that will be called |
|
current_pos = ahrs:get_position() |
|
|
|
-- adds new/extra mission item at the end by copying the last one and modifying it |
|
-- get number of last mission item |
|
wp_num = mission:num_commands()-1 |
|
--get last item from mission |
|
m = mission:get_item(wp_num) |
|
-- get first item from mission |
|
m1 = mission:get_item(1) |
|
|
|
if wp_num > 0 then |
|
gcs:send_text(0, string.format("LUA - Please clear misn to continue demo. size:%d",wp_num+1)) |
|
return update, 1000 |
|
end |
|
|
|
-- no mission, just home at [0] means user has cleared any mission in the system, and this demo is clear to write something new. |
|
-- it's not required that the mission be empty before we do things, but this script is multi-stage demo so its conveneient |
|
if ( mission:num_commands() == 1) then |
|
if demostage == 0 then |
|
demostage = 1 |
|
gcs:send_text(0, string.format("LUA demo stage 1 starting")) |
|
return stage1, 1000 |
|
end |
|
if demostage == 2 then |
|
demostage = 3 |
|
gcs:send_text(0, string.format("LUA demo stage 3 starting")) |
|
return stage3, 1000 |
|
end |
|
if demostage == 4 then |
|
demostage = 5 |
|
gcs:send_text(0, string.format("LUA demo stage 5 starting")) |
|
return stage5, 1000 |
|
end |
|
if demostage == 6 then |
|
demostage = 7 |
|
gcs:send_text(0, string.format("LUA demo stage 7 starting")) |
|
return stage7, 1000 |
|
end |
|
if demostage == 8 then |
|
demostage = 9 |
|
gcs:send_text(0, string.format("LUA MISSION demo all COMPLETED.")) |
|
--return update, 1000 |
|
end |
|
end |
|
return update, 1000 |
|
end |
|
|
|
function read_table_from_sd() |
|
-- Opens a file in read mode |
|
file = io.open("miss.txt", "r") |
|
-- sets the default input file as xxxx.txt |
|
io.input(file) |
|
|
|
-- read whole file, or get empty string |
|
content = io.read("*all") |
|
|
|
if (content == nil) then |
|
gcs:send_text(0, string.format("file not found, skipping read of miss.txt from sd")) |
|
return update(), 1000 |
|
end |
|
|
|
local pat = "wp:(%S+)%s+lat:(%S+)%s+lon:(%S+)%s+alt:(%S+)" |
|
for s1, s2 ,s3,s4 in string.gmatch(content, pat) do |
|
--s = string.format("wp:%s lat:%s lon:%s alt:%s",s1,s2,s3,s4) |
|
--gcs:send_text(0, s) |
|
n1 = tonumber(s1) |
|
n2 = math.floor(tonumber(s2*10000000)) |
|
n3 = math.floor(tonumber(s3*10000000)) |
|
n4 = tonumber(s4) |
|
|
|
-- use previous item as template... |
|
m = mission:get_item(mission:num_commands()-1) |
|
m:command(16) -- 16 = normal WAYPOINT |
|
m:x(n2) |
|
m:y(n3) |
|
m:z(n4) |
|
-- write as a new item to the end of the list. |
|
mission:set_item(mission:num_commands(),m) |
|
end |
|
|
|
gcs:send_text(0, '...loaded file from SD') |
|
|
|
-- closes the open file |
|
io.close(file) |
|
|
|
return update(), 1000 |
|
end |
|
|
|
function stage1 () |
|
-- demo stage 1 implementation. |
|
if (demostage == 1 ) and ( mission:num_commands() == 1 ) then |
|
demostage = 2 |
|
return read_table_from_sd(), 1000 |
|
end |
|
|
|
end |
|
|
|
function stage3 () |
|
-- demo stage 3 implementation. |
|
--get number of 'last' mission item |
|
wp_num = mission:num_commands()-1 |
|
-- get HOME item from mission as the 'reference' for future items |
|
m1 = mission:get_item(0) |
|
--get last item from mission |
|
m = mission:get_item(wp_num) |
|
if (demostage == 3 ) then |
|
-- demo stage 3 starts by writing 10 do-JUMPS over 10 seconds, just for fun |
|
if mission:num_commands() < 10 then |
|
m:command(177) -- 177 = DO_JUMP |
|
m:param1(m:param1()+1) -- some increments for fun/demo |
|
m:param2(m:param2()+1) |
|
gcs:send_text(0, string.format("LUA new miss-item DO_JUMP %d ", wp_num+1)) |
|
mission:set_item(mission:num_commands(),m) |
|
return stage3, 100 -- stay in stage 3 for now |
|
end |
|
|
|
-- change copy of last item slightly, for giggles and demo. |
|
-- This is reading a copy of whatever is currently the last item in the mission, do_jump |
|
-- and changing/ensuring its type is a 'normal' waypoint, and setting its lat/long/alt |
|
-- to data we earlier took from HOME, adding an offset and then writing it |
|
-- as a NEW mission item at the end |
|
if mission:num_commands() == 10 then |
|
m:command(16) -- 16 = normal WAYPOINT |
|
m:x(m1:x()+200) |
|
m:y(m1:y()+200) |
|
m:z(m1:z()+1) |
|
gcs:send_text(0, string.format("LUA new miss-item WAYPOINT a %d ", wp_num+1)) |
|
mission:set_item(mission:num_commands(),m) |
|
return stage3, 100 -- stay in stage 3 for now |
|
end |
|
|
|
-- change copy of last item slightly, for giggles, and append as a new item |
|
if (mission:num_commands() > 10) and (mission:num_commands() < 20) then |
|
m:command(16) -- 16 = normal WAYPOINT |
|
m:x(m:x()+200) |
|
m:y(m:y()+200) |
|
m:z(m:z()+1) |
|
gcs:send_text(0, string.format("LUA new miss-item WAYPOINT b %d ", wp_num+1)) |
|
mission:set_item(mission:num_commands(),m) |
|
return stage3, 100 -- stay in stage 3 for now |
|
end |
|
|
|
-- change copy of last item slightly, for giggles. |
|
if (mission:num_commands() >= 20) and (mission:num_commands() < 30) then |
|
m:command(16) -- 16 = normal WAYPOINT |
|
m:x(m:x()+200) |
|
m:y(m:y()+200) |
|
m:z(m:z()+1) |
|
gcs:send_text(0, string.format("LUA new miss-item WAYPOINT c %d ", wp_num+1)) |
|
mission:set_item(mission:num_commands(),m) |
|
return stage3, 100 -- stay in stage 3 for now |
|
end |
|
-- move on at end of this dempo stage |
|
if (mission:num_commands() >= 30) then |
|
gcs:send_text(0, string.format("LUA DEMO stage 3 done. ", wp_num+1)) |
|
demostage = 4 |
|
return update, 100 -- drop to next stage via an update() call |
|
end |
|
end |
|
|
|
end |
|
|
|
function stage5 () |
|
-- demo stage 5 implementation for when there's only one wp , HOME, in th system |
|
if (demostage == 5 ) then |
|
-- when no mission, uses home as reference point, otherwise its the 'last item' |
|
m = mission:get_item(mission:num_commands()-1) |
|
m:x(m:x()) |
|
m:y(m:y()-400) |
|
m:z(m:z()) |
|
mission:set_item(1,m) |
|
gcs:send_text(0, string.format("LUA mode 5 single wp nudge %d",eventcounter)) |
|
eventcounter = eventcounter+1 |
|
if eventcounter > 50 then |
|
demostage = 6 |
|
eventcounter = 0 |
|
gcs:send_text(0, string.format("LUA DEMO stage 5 done. ")) |
|
return update, 100 -- drop to next stage via an update() call |
|
end |
|
return stage5, 100 -- stay in stage 3 for now |
|
end |
|
|
|
end |
|
|
|
function stage7 () |
|
-- demo stage 5 implementation for when there's more than wp in the system |
|
if (demostage == 7 ) then --and (mission:num_commands() >= 3) and (mission:num_commands() < 50) then |
|
-- fiurst time in , there's no mission, lets throw a few wps in to play with later.. |
|
-- change copy of last item slightly, for giggles, and append as a new item |
|
if (mission:num_commands() == 1) then |
|
for x = 1, 10 do |
|
m:command(16) -- 16 = normal WAYPOINT |
|
m:x(m:x()+math.random(-10000,10000)) -- add something random |
|
m:y(m:y()+math.random(-10000,10000)) |
|
m:z(m:z()+1) |
|
gcs:send_text(0, string.format("LUA stage 7 making 10 new nearby random wp's %d ", wp_num+1)) |
|
mission:set_item(mission:num_commands(),m) |
|
end |
|
gcs:send_text(0, string.format("LUA scattering complete. %d ", wp_num+1)) |
|
return stage7, 100 -- stay in stage 3 for now |
|
end |
|
|
|
-- things that are further away from this one than distance X.. |
|
m1 = mission:get_item(1) |
|
|
|
-- leave item 0 alone, always |
|
for x = 1, mission:num_commands()-1 do |
|
|
|
mitem = mission:get_item(x) |
|
|
|
-- look at each mission item above 1, and get the distance from it to the copter. |
|
local target = Location() |
|
target:lat(mitem:x()) |
|
target:lng(mitem:y()) |
|
local cur_d = current_pos:get_distance(target) |
|
|
|
if cur_d > 100 then |
|
if mitem:x() > m1:x() then |
|
mitem:x(mitem:x()+400) |
|
end |
|
if mitem:x() < m1:x() then |
|
mitem:x(mitem:x()-400) |
|
end |
|
if mitem:y() > m1:y() then |
|
mitem:y(mitem:y()+400) |
|
end |
|
if mitem:y() < m1:y() then |
|
mitem:y(mitem:y()-400) |
|
end |
|
end |
|
|
|
-- write as a new item to the end of the list. |
|
mission:set_item(x,mitem) |
|
end |
|
gcs:send_text(0, string.format("LUA mode 7 scattering existing wp's.. %d",eventcounter)) |
|
end |
|
|
|
-- do it 50 times then consider it done |
|
eventcounter = eventcounter+1 |
|
if eventcounter > 50 then |
|
demostage = 8 |
|
eventcounter = 0 |
|
gcs:send_text(0, string.format("LUA DEMO stage 7 done. ")) |
|
return update, 100 -- drop to next stage via an update() call |
|
end |
|
|
|
return stage7, 500 |
|
end |
|
|
|
function wait_for_home() |
|
current_pos = ahrs:get_position() |
|
if current_pos == nil then |
|
return wait_for_home, 1000 |
|
end |
|
|
|
home = ahrs:get_home() |
|
if home == nil then |
|
return wait_for_home, 1000 |
|
end |
|
if home:lat() == 0 then |
|
return wait_for_home, 1000 |
|
end |
|
|
|
gcs:send_text(0, string.format("LUA MISSION Waiting for Home.")) |
|
|
|
return update, 1000 |
|
end |
|
|
|
function delayed_boot() |
|
gcs:send_text(0, string.format("LUA MISSION DEMO START")) |
|
return wait_for_home, 1000 |
|
end |
|
return delayed_boot, 5000 |
|
|
|
|