From b8042d57de6b6a6ce2f8f98aacf2e102429d100f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 20 Sep 2022 13:55:03 +0900 Subject: [PATCH] AP_Scripting: add mount-poi example --- libraries/AP_Scripting/examples/mount-poi.lua | 187 ++++++++++++++++++ 1 file changed, 187 insertions(+) create mode 100644 libraries/AP_Scripting/examples/mount-poi.lua diff --git a/libraries/AP_Scripting/examples/mount-poi.lua b/libraries/AP_Scripting/examples/mount-poi.lua new file mode 100644 index 0000000000..66995f590c --- /dev/null +++ b/libraries/AP_Scripting/examples/mount-poi.lua @@ -0,0 +1,187 @@ +-- 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 +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") + +local poi_dist_max = Parameter() +assert(poi_dist_max:init("POI_DIST_MAX"), "could not find POI_DIST_MAX param") + +-- bind to other parameters this script depends upon +TERRAIN_SPACING = Parameter("TERRAIN_SPACING") + +-- local variables and definitions +local user_update_interval_ms = 10000 -- send user updates every 10 sec +local last_user_update_ms = 0 -- system time that update was last sent to user +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() + + -- get current system time + local now_ms = millis() + + -- 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 + gcs:send_text(3, 'MountPOI: RCx_OPTION = 300 not set') -- MAV_SEVERITY_ERROR + 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 + gcs:send_text(3, "POI: vehicle pos unavailable") -- MAV_SEVERITY_ERROR + return update, UPDATE_INTERVAL_MS + end + + -- change vehicle location to ASML + vehicle_loc:change_alt_frame(ALT_FRAME_ABSOLUTE) + + -- retrieve gimbal attitude + local roll_deg, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0) + if pitch_deg == nil or yaw_bf_deg == nil then + gcs:send_text(3, "POI: gimbal attitude unavailable") -- MAV_SEVERITY_ERROR + 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(3, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR + 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(3, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR + return update, UPDATE_INTERVAL_MS + end + end + + -- check for errors + if (total_dist >= dist_max) then + gcs:send_text(3, "POI: unable to find terrain within " .. tostring(dist_max) .. " m") + elseif not terrain_amsl_m then + gcs:send_text(3, "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) + local poi_terr_asml_m = terrain:height_amsl(poi_loc, true) + gcs:send_text(6, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01)) + end + + return update, UPDATE_INTERVAL_MS +end + +return update()