-- 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 -- 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()