AP_Scripting: fix mount-poi script check errors
remove unused variables re-enable script checks
This commit is contained in:
parent
3ab63cc6b1
commit
f40a939a0c
@ -18,7 +18,6 @@
|
||||
-- 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
|
||||
-- luacheck: only 0
|
||||
|
||||
-- global definitions
|
||||
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
||||
@ -43,8 +42,6 @@ local POI_DIST_MAX = Parameter("POI_DIST_MAX")
|
||||
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
|
||||
@ -88,9 +85,6 @@ 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
|
||||
@ -123,7 +117,7 @@ function update()
|
||||
vehicle_loc:change_alt_frame(ALT_FRAME_ABSOLUTE)
|
||||
|
||||
-- retrieve gimbal attitude
|
||||
local roll_deg, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0)
|
||||
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") -- MAV_SEVERITY_ERROR
|
||||
return update, UPDATE_INTERVAL_MS
|
||||
@ -185,7 +179,6 @@ function update()
|
||||
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(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))
|
||||
end
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user