2022-09-20 01:55:03 -03:00
-- 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 to enable triggering the POI calculation from an auxiliary switch
-- 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
-- global definitions
2023-04-11 04:30:36 -03:00
local MAV_SEVERITY = { EMERGENCY = 0 , ALERT = 1 , CRITICAL = 2 , ERROR = 3 , WARNING = 4 , NOTICE = 5 , INFO = 6 , DEBUG = 7 }
2022-09-20 01:55:03 -03:00
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 " )
2023-04-11 04:30:36 -03:00
--[[
// @ 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 " )
2022-09-20 01:55:03 -03:00
-- bind to other parameters this script depends upon
TERRAIN_SPACING = Parameter ( " TERRAIN_SPACING " )
-- local variables and definitions
local last_rc_switch_pos = 0 -- last known rc switch position. Used to detect change in RC switch position
-- 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
-- the main update function that performs a simplified version of RTL
function update ( )
-- find RC channel used to trigger POI
rc_switch_ch = rc : find_channel_for_option ( 300 ) --scripting ch 1
if ( rc_switch_ch == nil ) then
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , ' MountPOI: RCx_OPTION = 300 not set ' ) -- MAV_SEVERITY_ERROR
2022-09-20 01:55:03 -03:00
return update , 10000 -- check again in 10 seconds
end
-- check if user has raised RC switch
local rc_switch_pos = rc_switch_ch : get_aux_switch_pos ( )
if rc_switch_pos == last_rc_switch_pos then
return update , UPDATE_INTERVAL_MS
end
-- switch has changed position
last_rc_switch_pos = rc_switch_pos
if rc_switch_pos ~= 2 then
return update , UPDATE_INTERVAL_MS
end
-- POI has been requested
-- retrieve vehicle location
local vehicle_loc = ahrs : get_location ( )
if vehicle_loc == nil then
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: vehicle pos unavailable " ) -- MAV_SEVERITY_ERROR
2022-09-20 01:55:03 -03:00
return update , UPDATE_INTERVAL_MS
end
-- change vehicle location to ASML
vehicle_loc : change_alt_frame ( ALT_FRAME_ABSOLUTE )
-- retrieve gimbal attitude
2023-04-11 04:38:47 -03:00
local _ , pitch_deg , yaw_bf_deg = mount : get_attitude_euler ( 0 )
2022-09-20 01:55:03 -03:00
if pitch_deg == nil or yaw_bf_deg == nil then
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: gimbal attitude unavailable " ) -- MAV_SEVERITY_ERROR
2022-09-20 01:55:03 -03:00
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
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: failed to get terrain alt " ) -- MAV_SEVERITY_ERROR
2022-09-20 01:55:03 -03:00
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
2023-04-11 04:30:36 -03:00
local dist_max = POI_DIST_MAX : get ( )
2022-09-20 01:55:03 -03:00
-- 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
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: failed to get terrain alt " ) -- MAV_SEVERITY_ERROR
2022-09-20 01:55:03 -03:00
return update , UPDATE_INTERVAL_MS
end
end
-- check for errors
if ( total_dist >= dist_max ) then
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: unable to find terrain within " .. tostring ( dist_max ) .. " m " )
2022-09-20 01:55:03 -03:00
elseif not terrain_amsl_m then
2023-04-11 04:30:36 -03:00
gcs : send_text ( MAV_SEVERITY.ERROR , " POI: failed to retrieve terrain alt " )
2022-09-20 01:55:03 -03:00
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 )
2023-04-11 04:30:36 -03:00
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 ) )
2022-09-20 01:55:03 -03:00
end
return update , UPDATE_INTERVAL_MS
end
return update ( )