rmackay9
commented
3 years ago 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.
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 EK3SRCn 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()