AP_Scripting: add test samples of get_target_location/update_target_location for copter

This commit is contained in:
TsuyoshiKawamura 2024-01-18 01:26:48 +09:00 committed by Randy Mackay
parent 0034a7fc30
commit b9bf7acbf1
2 changed files with 62 additions and 0 deletions

View File

@ -0,0 +1,16 @@
-- test get_target_location functions for copter
-- https://discuss.ardupilot.org/t/vehicle-get-target-location-in-lua-copter/108901
function update()
local next_WP = vehicle:get_target_location()
if not next_WP then
-- not in a flight mode with a target location
gcs:send_text(0,"not in a flight mode with target loc")
else
gcs:send_text(0,"target loc : " .. next_WP:lat() .. " ; " .. next_WP:lng())
end
return update, 10000
end
return update()

View File

@ -0,0 +1,46 @@
-- 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()