mirror of https://github.com/ArduPilot/ardupilot
275 lines
10 KiB
Lua
275 lines
10 KiB
Lua
-- mount-poi.lua: finds the point-of-interest that the gimbal mount is pointing at using the vehicle's current Location, mount attitude and terrain database
|
|
--
|
|
-- How To Use
|
|
-- 1. Set RCx_OPTION to 300 or 301 to enable triggering the POI calculation from an auxiliary switch. If 301 is used the gimbal will also lock onto the location
|
|
-- 2. optionally set POI_DIST_MAX to the maximum distance (in meters) that the POI point could be from the vehicle
|
|
-- 3. fly the vehicle and point the camera gimbal at a point on the ground
|
|
-- 4. raise the RC auxiliary switch and check the GCS's messages tab for the latitude, longitude and alt (above sea-level)
|
|
--
|
|
-- How It Works
|
|
-- 1. retrieve the POI_DIST_MAX and TERRAIN_SPACING param values
|
|
-- 2. get the vehicle Location (lat, lon, height above sea-level), initialise test-loc and prev-test-loc
|
|
-- 3. get the vehicle's current alt-above-terrain
|
|
-- 4. get gimbal attitude (only pitch and yaw are used)
|
|
-- 5. "test_loc" is initialised to the vehicle's location
|
|
-- 6. "prev_test_loc" is a backup of test_loc
|
|
-- 7. test_loc is moved along the line defined by the gimbal's pitch and yaw by TERRAIN_SPACING (meters)
|
|
-- 8. retrieve the terrain's altitude (above sea-level) at the test_loc
|
|
-- 9. repeat step 6, 7 and 8 until the test_loc's altitude falls below the terrain altitude
|
|
-- 10. interpolate between test_loc and prev_test_loc to find the lat, lon, alt (above sea-level) where alt-above-terrain is zero
|
|
-- 11. display the POI to the user
|
|
|
|
---@diagnostic disable: param-type-mismatch
|
|
---@diagnostic disable: cast-local-type
|
|
---@diagnostic disable: missing-parameter
|
|
|
|
-- global definitions
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
local ALT_FRAME_ABSOLUTE = 0
|
|
local UPDATE_INTERVAL_MS = 100
|
|
|
|
-- add new param POI_DIST_MAX
|
|
local PARAM_TABLE_KEY = 78
|
|
assert(param:add_table(PARAM_TABLE_KEY, "POI_", 1), "could not add param table")
|
|
assert(param:add_param(PARAM_TABLE_KEY, 1, "DIST_MAX", 10000), "could not add POI_DIST_MAX param")
|
|
|
|
--[[
|
|
// @Param: POI_DIST_MAX
|
|
// @DisplayName: Mount POI distance max
|
|
// @Description: POI's max distance (in meters) from the vehicle
|
|
// @Range: 0 10000
|
|
// @User: Standard
|
|
--]]
|
|
local POI_DIST_MAX = Parameter("POI_DIST_MAX")
|
|
|
|
-- bind to other parameters this script depends upon
|
|
TERRAIN_SPACING = Parameter("TERRAIN_SPACING")
|
|
|
|
-- local variables and definitions
|
|
local last_poi_switch_pos = 0 -- last known rc poi switch position. Used to detect change in RC switch position
|
|
local last_roi_switch_pos = 0 -- last known rc roi switch position. Used to detect change in RC switch position
|
|
local success_count = 0 -- count of the number of POI calculations (sent to GCS in CAMERA_FEEDBACK message)
|
|
|
|
-- mavlink message definition
|
|
-- initialise mavlink rx with number of messages, and buffer depth
|
|
mavlink.init(1, 10)
|
|
local messages = {}
|
|
messages[180] = { -- CAMERA_FEEDBACK
|
|
{ "time_usec", "<I8" },
|
|
{ "lat", "<i4" },
|
|
{ "lng", "<i4" },
|
|
{ "alt_msl", "<f" },
|
|
{ "alt_rel", "<f" },
|
|
{ "roll", "<f" },
|
|
{ "pitch", "<f" },
|
|
{ "yaw", "<f" },
|
|
{ "foc_len", "<f" },
|
|
{ "img_idx", "<I2" },
|
|
{ "target_system", "<B" },
|
|
{ "cam_idx", "<B" },
|
|
{ "flags", "<B" },
|
|
{ "completed_captures", "<I2" },
|
|
}
|
|
|
|
function encode(msgid, message, messages_array)
|
|
local message_map = messages_array[msgid]
|
|
if not message_map then
|
|
-- we don't know how to encode this message, bail on it
|
|
error("Unknown MAVLink message " .. msgid)
|
|
end
|
|
|
|
local packString = "<"
|
|
local packedTable = {}
|
|
local packedIndex = 1
|
|
for i,v in ipairs(message_map) do
|
|
if v[3] then
|
|
packString = (packString .. string.rep(string.sub(v[2], 2), v[3]))
|
|
for j = 1, v[3] do
|
|
packedTable[packedIndex] = message[message_map[i][1]][j]
|
|
packedIndex = packedIndex + 1
|
|
end
|
|
else
|
|
packString = (packString .. string.sub(v[2], 2))
|
|
packedTable[packedIndex] = message[message_map[i][1]]
|
|
packedIndex = packedIndex + 1
|
|
end
|
|
end
|
|
|
|
return string.pack(packString, table.unpack(packedTable))
|
|
end
|
|
|
|
-- send CAMERA_FEEDBACK message to GCS
|
|
function send_camera_feedback(lat_degE7, lon_degE7, alt_msl_m, alt_rel_m, roll_deg, pitch_deg, yaw_deg, foc_len_mm, feedback_flags, captures_count)
|
|
-- prepare camera feedback msg
|
|
local camera_feedback_msg = {
|
|
time_usec = micros():toint(),
|
|
target_system = 0,
|
|
cam_idx = 0,
|
|
img_idx = 1,
|
|
lat = lat_degE7,
|
|
lng = lon_degE7,
|
|
alt_msl = alt_msl_m,
|
|
alt_rel = alt_rel_m,
|
|
roll = roll_deg,
|
|
pitch = pitch_deg,
|
|
yaw = yaw_deg,
|
|
foc_len = foc_len_mm,
|
|
flags = feedback_flags,
|
|
completed_captures = captures_count
|
|
}
|
|
|
|
-- send camera feedback msg
|
|
local encoded_msg = encode(180, camera_feedback_msg, messages)
|
|
mavlink.send_chan(0, 180, encoded_msg)
|
|
mavlink.send_chan(1, 180, encoded_msg)
|
|
end
|
|
|
|
-- helper functions
|
|
function wrap_360(angle_deg)
|
|
local res = math.fmod(angle_deg, 360.0)
|
|
if res < 0 then
|
|
res = res + 360.0
|
|
end
|
|
return res
|
|
end
|
|
|
|
function wrap_180(angle_deg)
|
|
local res = wrap_360(angle_deg)
|
|
if res > 180 then
|
|
res = res - 360
|
|
end
|
|
return res
|
|
end
|
|
|
|
function swap_float(f1, f2)
|
|
return f2, f1
|
|
end
|
|
|
|
function interpolate(low_output, high_output, var_value, var_low, var_high)
|
|
-- support either polarity
|
|
if (var_low > var_high) then
|
|
var_low, var_high = swap_float(var_low, var_high)
|
|
low_output, high_output = swap_float(low_output, high_output)
|
|
end
|
|
if (var_value <= var_low) then
|
|
return low_output
|
|
end
|
|
if (var_value > var_high) then
|
|
return high_output
|
|
end
|
|
local p = (var_value - var_low) / (var_high - var_low)
|
|
return (low_output + p * (high_output - low_output))
|
|
end
|
|
|
|
gcs:send_text(MAV_SEVERITY.INFO, "Mount-poi script started")
|
|
|
|
-- the main update function called at 10hz
|
|
function update()
|
|
|
|
-- check if user has raised POI switch
|
|
local poi_switch_pos = rc:get_aux_cached(300) -- scripting ch 1 (drop icon on map where mount is pointing)
|
|
local poi_switch_pulled_high = (poi_switch_pos ~= nil) and (poi_switch_pos ~= last_poi_switch_pos) and (poi_switch_pos == 2)
|
|
last_poi_switch_pos = poi_switch_pos
|
|
|
|
-- check if user has raised ROI switch
|
|
local roi_switch_pos = rc:get_aux_cached(301) -- scripting ch 2 (drop icon and lock mount on location)
|
|
local roi_switch_pulled_high = (roi_switch_pos ~= nil) and (roi_switch_pos ~= last_roi_switch_pos) and (roi_switch_pos == 2)
|
|
last_roi_switch_pos = roi_switch_pos
|
|
|
|
-- return if neither switch was pulled high
|
|
if not poi_switch_pulled_high and not roi_switch_pulled_high then
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
-- POI or ROI has been requested
|
|
|
|
-- retrieve vehicle location
|
|
local vehicle_loc = ahrs:get_location()
|
|
if vehicle_loc == nil then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: vehicle pos unavailable")
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
-- change vehicle location to ASML
|
|
vehicle_loc:change_alt_frame(ALT_FRAME_ABSOLUTE)
|
|
|
|
-- retrieve gimbal attitude
|
|
local _, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0)
|
|
if pitch_deg == nil or yaw_bf_deg == nil then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: gimbal attitude unavailable")
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
-- project forward from vehicle looking for terrain
|
|
-- start testing at vehicle's location
|
|
local test_loc = vehicle_loc:copy()
|
|
local prev_test_loc = test_loc:copy()
|
|
|
|
-- get terrain altitude (asml) at test_loc
|
|
local terrain_amsl_m = terrain:height_amsl(test_loc, true) -- terrain alt (above amsl) at test_loc
|
|
local prev_terrain_amsl_m = terrain_amsl_m -- terrain alt (above amsl) at prev_test_loc
|
|
|
|
-- fail if terrain alt cannot be retrieved
|
|
if terrain_amsl_m == nil then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt")
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
-- get gimbal mount's pitch and yaw
|
|
local mount_pitch_deg = pitch_deg
|
|
local mount_yaw_ef_deg = wrap_180(yaw_bf_deg + math.deg(ahrs:get_yaw()))
|
|
local dist_increment_m = TERRAIN_SPACING:get()
|
|
|
|
-- initialise total distance test_loc has moved
|
|
local total_dist = 0
|
|
local dist_max = POI_DIST_MAX:get()
|
|
|
|
-- iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
|
|
while (total_dist < dist_max) and ((test_loc:alt() * 0.01) > terrain_amsl_m) do
|
|
total_dist = total_dist + dist_increment_m
|
|
|
|
-- take backup of previous test location and terrain asml
|
|
prev_test_loc = test_loc:copy()
|
|
prev_terrain_amsl_m = terrain_amsl_m
|
|
|
|
-- move test location forward
|
|
test_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m)
|
|
|
|
-- get terrain's alt-above-sea-level (at test_loc)
|
|
terrain_amsl_m = terrain:height_amsl(test_loc, true)
|
|
|
|
-- fail if terrain alt cannot be retrieved
|
|
if terrain_amsl_m == nil then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt")
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
end
|
|
|
|
-- check for errors
|
|
if (total_dist >= dist_max) then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: unable to find terrain within " .. tostring(dist_max) .. " m")
|
|
elseif not terrain_amsl_m then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to retrieve terrain alt")
|
|
else
|
|
-- test location has dropped below terrain
|
|
-- interpolate along line between prev_test_loc and test_loc
|
|
local dist_interp_m = interpolate(0, dist_increment_m, 0, prev_test_loc:alt() * 0.01 - prev_terrain_amsl_m, test_loc:alt() * 0.01 - terrain_amsl_m)
|
|
local poi_loc = prev_test_loc:copy()
|
|
poi_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m)
|
|
gcs:send_text(MAV_SEVERITY.INFO, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01))
|
|
|
|
-- if ROI requested then also lock gimbal to location
|
|
if roi_switch_pulled_high then
|
|
mount:set_roi_target(0, poi_loc)
|
|
end
|
|
|
|
-- send feedback to GCS so it can display icon on map
|
|
success_count = success_count + 1
|
|
send_camera_feedback(poi_loc:lat(), poi_loc:lng(), poi_loc:alt(), poi_loc:alt(), 0, 0, 0, 0, 0, success_count)
|
|
end
|
|
|
|
return update, UPDATE_INTERVAL_MS
|
|
end
|
|
|
|
return update()
|