From 68274ff996dc7f92e3cbdbda63d0dc2297bc0733 Mon Sep 17 00:00:00 2001 From: tajisoft Date: Mon, 11 Oct 2021 13:09:20 +0900 Subject: [PATCH] AP_Scripting: fix switch check in ahrs-source --- .../examples/ahrs-source-gps-optflow.lua | 53 ++++++++++--------- .../ahrs-source-gps-wheelencoders.lua | 51 +++++++++--------- .../AP_Scripting/examples/ahrs-source.lua | 51 +++++++++--------- 3 files changed, 82 insertions(+), 73 deletions(-) diff --git a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua index 7c93cdf247..9d33b4bd09 100644 --- a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua +++ b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua @@ -64,11 +64,12 @@ end function update() -- check switches are configured + -- at least one switch must be set -- 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 not rc_function_source or not rc_function_auto then + if not rc_function_source and not rc_function_auto then gcs:send_text(0, "ahrs-source-gps-optflow.lua: RCx_FUNCTION=90 or 300 not set!") return update, 1000 end @@ -166,32 +167,34 @@ function update() 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)) + -- if auto switch exists then read auto source switch position from RCx_FUNCTION = 300 (Scripting1) + if rc_function_auto then + 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 - play_source_tune(source_prev) end -- auto switching diff --git a/libraries/AP_Scripting/examples/ahrs-source-gps-wheelencoders.lua b/libraries/AP_Scripting/examples/ahrs-source-gps-wheelencoders.lua index b9a211c2cf..af60326492 100644 --- a/libraries/AP_Scripting/examples/ahrs-source-gps-wheelencoders.lua +++ b/libraries/AP_Scripting/examples/ahrs-source-gps-wheelencoders.lua @@ -35,11 +35,12 @@ end function update() -- check switches are configured + -- at least one switch must be set -- 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 + if (rc_function_source == nil) and (rc_function_auto == nil) then gcs:send_text(0, "ahrs-source-gps-wheelencoders.lua: RCx_FUNCTION=90 or 300 not set!") return update, 1000 end @@ -102,31 +103,33 @@ function update() 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)) + if rc_function_auto then + 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 - play_source_tune(source_prev) end -- auto switching diff --git a/libraries/AP_Scripting/examples/ahrs-source.lua b/libraries/AP_Scripting/examples/ahrs-source.lua index f2bcf58776..6acf4c8bb7 100644 --- a/libraries/AP_Scripting/examples/ahrs-source.lua +++ b/libraries/AP_Scripting/examples/ahrs-source.lua @@ -42,11 +42,12 @@ end function update() -- check switches are configured + -- at least one switch must be set -- 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 + if (rc_function_source == nil) and (rc_function_auto == nil) then gcs:send_text(0, "ahrs-source.lua: RCx_FUNCTION=90 or 300 not set!") return update, 1000 end @@ -140,31 +141,33 @@ function update() 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)) + if rc_function_auto then + 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 - play_source_tune(source_prev) end -- auto switching