mirror of https://github.com/ArduPilot/ardupilot
67 lines
2.1 KiB
Lua
67 lines
2.1 KiB
Lua
|
-- This script will select a random location (as defined from a lat long coordinate)
|
||
|
|
-- and will play a set of tunes to navigate you towards that point, and select a new once
|
||
|
|
-- once the point has been found.
|
||
|
|
--
|
||
|
|
-- This script primarily serves to demo how to work with Locations, as well as how to
|
||
|
|
-- use the tonealarm to play tones, and send status text messages
|
||
|
|
|
||
|
|
local ACCEPTANCE_DISTANCE = 20.0
|
||
|
|
|
||
|
|
local TUNE_POINT = "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8"
|
||
|
|
local TUNE_TOWARDS = "MFT100L8>B"
|
||
|
|
local TUNE_AWAY = "MFT100L4>A#B#"
|
||
|
|
|
||
|
|
local target = Location()
|
||
|
|
local top_left = Location()
|
||
|
|
top_left:lat(-353622666)
|
||
|
|
top_left:lng(1491650479)
|
||
|
|
|
||
|
|
local last_distance = 1e10
|
||
|
|
local notify_interval_ms = uint32_t(5000)
|
||
|
|
local last_notify_time_ms = millis()
|
||
|
|
local score = 0
|
||
|
|
|
||
|
|
function find_next_point ()
|
||
|
|
target:lat(top_left:lat())
|
||
|
|
target:lng(top_left:lng())
|
||
|
|
target:offset(math.random()*-100, math.random()*10)
|
||
|
|
gcs:send_text(6, string.format("New target %d %d", target:lat(), target:lng()))
|
||
|
|
local current = ahrs:get_position()
|
||
|
|
if current then
|
||
|
|
last_distance = current:get_distance(target)
|
||
|
|
end
|
||
|
|
last_distance = 1e10
|
||
|
|
return
|
||
|
|
end
|
||
|
|
|
||
|
|
function update ()
|
||
|
|
local current = ahrs:get_position()
|
||
|
|
if current then
|
||
|
|
local dist = target:get_distance(current)
|
||
|
|
local now = millis()
|
||
|
|
if dist < ACCEPTANCE_DISTANCE then
|
||
|
|
notify:play_tune(TUNE_POINT)
|
||
|
|
score = score + 1
|
||
|
|
gcs:send_text(6, string.format("Got a point! %d total", score))
|
||
|
|
find_next_point()
|
||
|
|
elseif (now - last_notify_time_ms) > notify_interval_ms then
|
||
|
|
last_notify_time_ms = now
|
||
|
|
gcs:send_text(6, string.format("Distance: %.1f %.1f", target:get_distance(current), dist))
|
||
|
|
if dist < (last_distance - 1) then
|
||
|
|
notify:play_tune(TUNE_TOWARDS)
|
||
|
|
elseif dist > (last_distance + 1) then
|
||
|
|
notify:play_tune(TUNE_AWAY)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
if math.abs(last_distance - dist) > 1.0 then
|
||
|
|
last_distance = dist;
|
||
|
|
end
|
||
|
|
|
||
|
|
end
|
||
|
|
return update, 100
|
||
|
|
end
|
||
|
|
|
||
|
|
find_next_point()
|
||
|
|
|
||
|
|
return update, 100
|