mirror of https://github.com/ArduPilot/ardupilot
47 lines
1.3 KiB
Lua
47 lines
1.3 KiB
Lua
|
-- test update_target_location functions for copter
|
||
|
|
|
||
|
|
local current_target = nil
|
||
|
|
local cur_loc = nil
|
||
|
|
local new_target= nil
|
||
|
|
|
||
|
|
function update()
|
||
|
|
|
||
|
|
if not (vehicle:get_mode() == 4) then
|
||
|
|
gcs:send_text(0, "not in Guided")
|
||
|
|
return update, 1000
|
||
|
|
end
|
||
|
|
|
||
|
|
current_target = vehicle:get_target_location()
|
||
|
|
if not current_target then
|
||
|
|
return update, 1000
|
||
|
|
end
|
||
|
|
|
||
|
|
gcs:send_text(6, string.format("Current target %d %d %d frame %d", current_target:lat(), current_target:lng(), current_target:alt(), current_target:get_alt_frame()))
|
||
|
|
|
||
|
|
cur_loc = ahrs:get_position()
|
||
|
|
if not cur_loc then
|
||
|
|
gcs:send_text(0, "current position is not good")
|
||
|
|
return update, 1000
|
||
|
|
end
|
||
|
|
|
||
|
|
gcs:send_text(6, string.format("alt is %f", cur_loc:alt()*0.01))
|
||
|
|
if cur_loc:alt()*0.01 < 650 then
|
||
|
|
gcs:send_text(0, "too low")
|
||
|
|
return update, 1000
|
||
|
|
end
|
||
|
|
|
||
|
|
-- just add some offset to current location
|
||
|
|
new_target = cur_loc:copy()
|
||
|
|
new_target:lat(cur_loc:lat() + 1000)
|
||
|
|
new_target:lng(cur_loc:lng() + 1000)
|
||
|
|
new_target:alt(cur_loc:alt() + 1000)
|
||
|
|
|
||
|
|
gcs:send_text(6, string.format("New target %d %d %d frame %d", new_target:lat(), new_target:lng(), new_target:alt(), new_target:get_alt_frame()))
|
||
|
|
|
||
|
|
vehicle:update_target_location(current_target, new_target)
|
||
|
|
|
||
|
|
return update, 2000
|
||
|
|
end
|
||
|
|
|
||
|
|
return update()
|