How to switch automatically between GPS and optical flow

[diegodab]

The following program is written in Lua and works for GPS + a camera like the T265 + optical flow. It would be absolutely extremely helpful.

-- switches between AHRS/EKF sources based on the pilot's source selection switch or using an automatic source selection algorithm
-- this script is intended to help vehicles move between GPS and Non-GPS environments

-- setup RCx_OPTION = 90 (EKF Pos Source) to select the source (low=primary, middle=secondary, high=tertiary)
-- setup RCx_OPTION = 300 (Scripting1). When this switch is pulled high, the source will be automatically selected
-- setup EK3_SRCn_ parameters so that GPS is the primary source, Non-GPS (i.e. T265) is secondary and optical flow tertiary
-- configure a forward or downward facing lidar with a range of more than 5m

-- When the auxiliary switch (ZigZag Auto) is pulled high automatic source selection uses these thresholds:
-- SCR_USER1 holds the threshold for rangefinder altitude:
-- if rangefinder distance >= SCR_USER1, source2 (ExtNav) will be used (if Non-GPS vel innovations are good)
-- if rangefinder distance < SCR_USER1, source3 (optical flow) will be used (if Non-GPS vel innovations are not good)
-- SCR_USER2 holds the threshold for GPS speed accuracy (around 0.3 is a good choice)
-- SCR_USER3 holds the threshold for Non-GPS vertical speed innovation (about 0.3 is a good choice)
-- if both GPS speed accuracy <= SCR_USER2 and ExternalNav speed variance >= SCR_USER3, source1 will be used
-- otherwise source2 (T265) or source3 (optical flow) will be used based on rangefinder distance

local rangefinder_rotation = 25 -- check downward (25) facing lidar
local source_prev = 0 -- previous source, defaults to primary source
local sw_source_prev = -1 -- previous source switch position
local sw_auto_pos_prev = -1 -- previous auto source switch position
local auto_switch = false -- true when auto switching between sources is active
local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value
local vote_counter_max = 20 -- when a vote counter reaches this number (i.e. 2sec) source may be switched
local gps_vs_nongps_vote = 0 -- vote counter for GPS vs NonGPS (-20 = GPS, +20 = NonGPS)
local extnav_vs_opticalflow_vote = 0 -- vote counter for extnav vs optical flow (-20 = extnav, +20 = opticalflow)

-- play tune on buzzer to alert user to change in active source set
function play_source_tune(source)
if (source) then
if (source == 0) then
notify:play_tune("L8C") -- one long lower tone
elseif (source == 1) then
notify:play_tune("L12DD") -- two fast medium tones
elseif (source == 2) then
notify:play_tune("L16FFF") -- three very fast, high tones
end
end
end

-- the main update function
function update()

-- check switches are configured
-- source selection from RCx_FUNCTION = 90 (EKF Source Select)
-- auto source from RCx_FUNCTION = 300 (Scripting1)
local rc_function_source = rc:find_channel_for_option(90)
local rc_function_auto = rc:find_channel_for_option(300)
if (rc_function_source == nil) or (rc_function_auto == nil) then
gcs:send_text(0, "ahrs-source.lua: RCx_FUNCTION=90 or 300 not set!")
return update, 1000
end

-- check rangefinder distance threshold has been set
local rangefinder_thresh_dist = param:get('SCR_USER1') -- SCR_USER1 holds rangefinder threshold
if (rangefinder_thresh_dist == nil) or (rangefinder_thresh_dist <= 0) then
gcs:send_text(0, "ahrs-source.lua: set SCR_USER1 to rangefinder threshold")
return update, 1000
end

-- check GPS speed accuracy threshold has been set
local gps_speedaccuracy_thresh = param:get('SCR_USER2') -- SCR_USER2 holds GPS speed accuracy threshold
if (gps_speedaccuracy_thresh == nil) or (gps_speedaccuracy_thresh <= 0) then
gcs:send_text(0, "ahrs-source.lua: set SCR_USER2 to GPS speed accuracy threshold")
return update, 1000
end

-- check external nav innovation threshold has been set
local extnav_innov_thresh = param:get('SCR_USER3') -- SCR_USER3 holds Non-GPS vertical velocity innovation
if (extnav_innov_thresh == nil) or (extnav_innov_thresh <= 0) then
gcs:send_text(0, "ahrs-source.lua: set SCR_USER3 to ExtNav innovation threshold")
return update, 1000
end

-- check if GPS speed accuracy is over threshold
local gps_speed_accuracy = gps:speed_accuracy(gps:primary_sensor())
local gps_over_threshold = (gps_speed_accuracy == nil) or (gps:speed_accuracy(gps:primary_sensor()) > gps_speedaccuracy_thresh)
local gps_usable = (gps_speed_accuracy ~= nil) and (gps_speed_accuracy <= gps_usable_accuracy)

-- get external nav innovations from ahrs
local extnav_innov = Vector3f()
local extnav_var = Vector3f()
extnav_innov, extnav_var = ahrs:get_vel_innovations_and_variances_for_source(6)
local extnav_over_threshold = (extnav_innov == nil) or (extnav_innov:z() == 0.0) or (math.abs(extnav_innov:z()) > extnav_innov_thresh)

-- get rangefinder distance
local rngfnd_distance_m = 0
if rangefinder:has_data_orient(rangefinder_rotation) then
rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01
end
local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > rangefinder_thresh_dist)

-- NonGPS is usable if extnav innovations are good or rangefinder distance is short (for optical flow)
local nongps_usable = (not extnav_over_threshold) or (not rngfnd_over_threshold)

-- automatic selection logic --

-- GPS vs NonGPS vote. "-1" to move towards GPS, "+1" to move to Non-GPS
if (not gps_over_threshold) or (gps_usable and not nongps_usable) then
-- vote for GPS if GPS accuracy good OR usable GPS and NonGPS unusable
gps_vs_nongps_vote = math.max(gps_vs_nongps_vote - 1, -vote_counter_max)
elseif nongps_usable then
-- vote for NonGPS if extnav or opticalflow is usable
gps_vs_nongps_vote = math.min(gps_vs_nongps_vote + 1, vote_counter_max)
end

-- extnav vs optical flow vote. "-1" to move towards extnav, "+1" to move to opticalflow
if (not extnav_over_threshold) then
-- vote for extnav is innovations under threshold
extnav_vs_opticalflow_vote = math.max(extnav_vs_opticalflow_vote - 1, -vote_counter_max)
elseif (not rngfnd_over_threshold) then
-- vote for optical flow if rangefinder is not over threshold
extnav_vs_opticalflow_vote = math.min(extnav_vs_opticalflow_vote + 1, vote_counter_max)
end

-- auto source vote collation
local auto_source = -1 -- auto source undecided if -1
if gps_vs_nongps_vote <= -vote_counter_max then
auto_source = 0 -- GPS
elseif gps_vs_nongps_vote >= vote_counter_max then
if extnav_vs_opticalflow_vote <= -vote_counter_max then
auto_source = 1 -- extnav
elseif extnav_vs_opticalflow_vote >= vote_counter_max then
auto_source = 2 -- opticalflow
end
end

-- read source switch position from RCx_FUNCTION = 90 (EKF Source Select)
local sw_source_pos = rc_function_source:get_aux_switch_pos()
if sw_source_pos ~= sw_source_pos_prev then -- check for changes in source switch position
sw_source_pos_prev = sw_source_pos -- record new switch position so we can detect changes
auto_switch = false -- disable auto switching of source
if source_prev ~= sw_source_pos then -- check if switch position does not match source (there is a one-to-one mapping of switch to source)
source_prev = sw_source_pos -- record what source should now be (changed by ArduPilot vehicle code)
gcs:send_text(0, "Pilot switched to Source " .. string.format("%d", source_prev+1))
else
gcs:send_text(0, "Pilot switched but already Source " .. string.format("%d", source_prev+1))
end
play_source_tune(source_prev) -- alert user of source whether changed or not
end

-- read auto source switch position from RCx_FUNCTION = 300 (Scripting1)
local sw_auto_pos = rc_function_auto:get_aux_switch_pos()
if sw_auto_pos ~= sw_auto_pos_prev then -- check for changes in source auto switch position
sw_auto_pos_prev = sw_auto_pos -- record new switch position so we can detect changes
if sw_auto_pos == 0 then -- pilot has pulled switch low
auto_switch = false -- disable auto switching of source
if sw_source_pos ~= source_prev then -- check if source will change
source_prev = sw_source_pos -- record pilot's selected source
ahrs:set_posvelyaw_source_set(source_prev) -- switch to pilot's selected source
gcs:send_text(0, "Auto source disabled, switched to Source " .. string.format("%d", source_prev+1))
else
gcs:send_text(0, "Auto source disabled, already Source " .. string.format("%d", source_prev+1))
end
elseif sw_auto_pos == 2 then -- pilot has pulled switch high
auto_switch = true -- enable auto switching of source
if auto_source < 0 then
gcs:send_text(0, "Auto source enabled, undecided, Source " .. string.format("%d", source_prev+1))
elseif auto_source ~= source_prev then -- check if source will change
source_prev = auto_source -- record pilot's selected source
ahrs:set_posvelyaw_source_set(source_prev) -- switch to pilot's selected source
gcs:send_text(0, "Auto source enabled, switched to Source " .. string.format("%d", source_prev+1))
else
gcs:send_text(0, "Auto source enabled, already Source " .. string.format("%d", source_prev+1))
end
end
play_source_tune(source_prev)
end

-- auto switching
if auto_switch and (auto_source >= 0) and (auto_source ~= source_prev) then
source_prev = auto_source -- record selected source
ahrs:set_posvelyaw_source_set(source_prev) -- switch to pilot's selected source
gcs:send_text(0, "Auto switched to Source " .. string.format("%d", source_prev+1))
play_source_tune(source_prev)
end

return update, 100
end

return update()

[rmackay9]

I think this was closed because we already have it on the to-do list to add a GPS+optical flow only source switching script... so this is a duplicate request.

Thanks very much though and I'll hopefully add that script soon. It's not too hard although ideally we need some feedback from the EKF on how big the optical flow data's innovations are.