-
Notifications
You must be signed in to change notification settings - Fork 17k
/
mount-poi.lua
274 lines (234 loc) · 10.2 KB
/
mount-poi.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
-- 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()