From 461c576de507aead47fff6d7283f5755eb34a4b7 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sat, 25 Nov 2023 21:05:45 -0800 Subject: [PATCH] AP_Scripting: Add LUA interface to access Range Finder signal_quality data --- libraries/AP_Scripting/docs/docs.lua | 14 +- .../examples/rangefinder_quality_test.lua | 257 ++++++++++++++++++ .../generator/description/bindings.desc | 5 +- libraries/AP_Scripting/lua_bindings.cpp | 23 ++ libraries/AP_Scripting/lua_bindings.h | 1 + 5 files changed, 297 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Scripting/examples/rangefinder_quality_test.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 40ddb7ac9c35fa..a93e9611f91e7c 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2429,10 +2429,11 @@ function terrain:enabled() end ---@class AP_RangeFinder_Backend_ud local AP_RangeFinder_Backend_ud = {} --- Send distance to lua rangefinder backend. Returns false if failed +-- Send distance and optionally signal_quality to lua rangefinder backend. Returns false if failed ---@param distance number +---@param signal_quality_pct number? ---@return boolean -function AP_RangeFinder_Backend_ud:handle_script_msg(distance) end +function AP_RangeFinder_Backend_ud:handle_script_msg(distance, signal_quality_pct) end -- Status of this rangefinder instance ---@return integer @@ -2450,6 +2451,10 @@ function AP_RangeFinder_Backend_ud:orientation() end ---@return number function AP_RangeFinder_Backend_ud:distance() end +-- Current distance measurement signal_quality of the sensor instance +---@return number +function AP_RangeFinder_Backend_ud:signal_quality() end + -- desc ---@class rangefinder rangefinder = {} @@ -2494,6 +2499,11 @@ function rangefinder:max_distance_cm_orient(orientation) end ---@return integer function rangefinder:distance_cm_orient(orientation) end +-- Current distance measurement signal quality for range finder at this orientation +---@param orientation integer +---@return integer +function rangefinder:signal_quality_pct_orient(orientation) end + -- desc ---@param orientation integer ---@return boolean diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua new file mode 100644 index 00000000000000..873f673b64cd55 --- /dev/null +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -0,0 +1,257 @@ + +-- This test uses the Range Finder driver interface to simulate Range Finder +-- hardware and uses the Range Finder client interface to simulate a +-- client of the driver. The test sends distance data through the driver +-- interface and validates that it can be read through the client interface. + +-- Parameters should be set as follows before this test is loaded. +-- "RNGFND1_TYPE": 36, +-- "RNGFND1_ORIENT": 25, +-- "RNGFND1_MIN_CM": 10, +-- "RNGFND1_MAX_CM": 5000, + +-- UPDATE_PERIOD_MS is the time between when a distance is set and +-- when it is read. There is a periodic task that copies the set distance to +-- the state structure that it is read from. If UPDATE_PERIOD_MS is too short this periodic +-- task might not get a chance to run. A value of 25 seems to be too quick for sub. +local UPDATE_PERIOD_MS = 50 +local TIMEOUT_MS = 5000 + +-- These strings must match the strings used by the test driver for interpreting the output from this test. +local TEST_ID_STR = "RQTL" +local COMPLETE_STR = "#COMPLETE#" +local SUCCESS_STR = "!!SUCCESS!!" +local FAILURE_STR = "!!FAILURE!!" + + +-- Copied from libraries/AP_Math/rotation.h enum Rotation {}. +local RNGFND_ORIENTATION_DOWN = 25 +local RNGFND_ORIENTATION_FORWARD = 0 +-- Copied from libraries/AP_RangeFinder/AP_RanggeFinder.h enum RangeFinder::Type {}. +local RNGFND_TYPE_LUA = 36.0 +-- Copied from libraries/AP_RangeFinder/AP_RangeFinder.h enum RangeFinder::Status {}. +local RNDFND_STATUS_NOT_CONNECTED = 0 +local RNDFND_STATUS_NO_DATA = 1 +local RNDFND_STATUS_OUT_OF_RANGE_LOW = 2 +local RNDFND_STATUS_OUT_OF_RANGE_HIGH = 3 +local RNDFND_STATUS_GOOD = 4 + + +local function send(str) + gcs:send_text(3, string.format("%s %s", TEST_ID_STR, str)) +end + + +-- The range finder backend is initialized in the update_prepare function +---@type AP_RangeFinder_Backend_ud +local rngfnd_backend + + +local function test_dist_equal(dist_m_in, dist_in_factor, dist_out, signal_quality_pct_in, signal_quality_pct_out) + if math.abs(dist_out - dist_m_in * dist_in_factor) > 1.0e-3 then + return false + end + if signal_quality_pct_in < 0 and signal_quality_pct_out == -1 then + return true + end + if signal_quality_pct_in > 100 and signal_quality_pct_out == -1 then + return true + end + if math.floor(signal_quality_pct_in) == signal_quality_pct_out then + return true + end + return false +end + +local function get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_expected) + local status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_DOWN) + + -- Check that the status is as expected + if status_expected ~= status_actual then + return string.format("Test %i Status incorrect - expected %i, actual %i", test_idx, status_expected, status_actual) + end + + -- Not more checks if the status is poor + if status_actual ~= RNDFND_STATUS_GOOD then + send(string.format("Test %i status correct - expected: %i actual: %i", test_idx, status_expected, status_actual)) + return nil + end + + -- Check that the distance and signal_quality from the frontend are as expected + local dist_cm_out = rangefinder:distance_cm_orient(RNGFND_ORIENTATION_DOWN) + local signal_quality_pct_out = rangefinder:signal_quality_pct_orient(RNGFND_ORIENTATION_DOWN) + + -- Make sure data was returned + if not dist_cm_out or not signal_quality_pct_out then + return "No data returned from rangefinder:distance_cm_orient()" + end + + send(string.format("Frontend test %i dist in_m: %.2f out_cm: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, dist_cm_out, signal_quality_pct_in, signal_quality_pct_out)) + + if not test_dist_equal(dist_m_in, 100.0, dist_cm_out, signal_quality_pct_in, signal_quality_pct_out) then + return "Frontend expected and actual do not match" + end + + -- Check that the distance and signal_quality from the backend are as expected + local dist_m_out = rngfnd_backend:distance() + local signal_quality_out = rngfnd_backend:signal_quality() + + send(string.format("Backend test %i dist in_m: %.2f out_m: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, dist_m_out, signal_quality_pct_in, signal_quality_out)) + + if not test_dist_equal(dist_m_in, 1.0, dist_m_out, signal_quality_pct_in, signal_quality_out) then + return "Backend expected and actual do not match" + end + + return nil +end + +-- Test various status states +local function do_status_tests() + send("Test initial status") + local status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_DOWN) + if status_actual ~= RNDFND_STATUS_NO_DATA then + return "Status not NO_DATA on initialization." + end + status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_FORWARD) + if status_actual ~= RNDFND_STATUS_NOT_CONNECTED then + return "Status not NOT_CONNECTED for unconnected orientation." + end + return nil +end + + +local test_data = { + {20.0, -1, RNDFND_STATUS_GOOD}, + {20.5, -2, RNDFND_STATUS_GOOD}, + {21.0, 0, RNDFND_STATUS_GOOD}, + {22.0, 50, RNDFND_STATUS_GOOD}, + {23.0, 100, RNDFND_STATUS_GOOD}, + {24.0, 101, RNDFND_STATUS_GOOD}, + {25.0, -3, RNDFND_STATUS_GOOD}, + {26.0, 10000, RNDFND_STATUS_GOOD}, + {27.0, 1.5, RNDFND_STATUS_GOOD}, + {28.0, -0.5, RNDFND_STATUS_GOOD}, + {29.0, 99.5, RNDFND_STATUS_GOOD}, + {100.0, 100, RNDFND_STATUS_OUT_OF_RANGE_HIGH}, + {0.0, 100, RNDFND_STATUS_OUT_OF_RANGE_LOW}, +} + +-- Record the start time so we can timeout if initialization takes too long. +local time_start_ms = millis():tofloat() +local test_idx = 0 + + +-- Called when tests are completed. +local function complete(error_str) + -- Send a message indicating the success or failure of the test + local status_str + if not error_str or #error_str == 0 then + status_str = SUCCESS_STR + else + status_str = string.format("%s - %s", FAILURE_STR, error_str) + end + send(string.format("%s: %s", COMPLETE_STR, status_str)) + + -- Returning nil will not queue an update routine so the test will stop running. +end + + +-- A state machine of update functions. The states progress: +-- prepare, wait, begin_test, eval_test, begin_test, eval_test, ... complete + +local update_prepare +local update_wait +local update_begin_test +local update_eval_test + +local function _update_prepare() + if Parameter('RNGFND1_TYPE'):get() ~= RNGFND_TYPE_LUA then + return complete("LUA range finder driver not enabled") + end + if rangefinder:num_sensors() < 1 then + return complete("LUA range finder driver not connected") + end + rngfnd_backend = rangefinder:get_backend(0) + if not rngfnd_backend then + return complete("Range Finder 1 does not exist") + end + if (rngfnd_backend:type() ~= RNGFND_TYPE_LUA) then + return complete("Range Finder 1 is not a LUA driver") + end + + return update_wait() +end + +local function _update_wait() + -- Check for timeout while initializing + if millis():tofloat() - time_start_ms > TIMEOUT_MS then + return complete("Timeout while trying to initialize") + end + + -- Wait until the prearm check passes. This ensures that the system is mostly initialized + -- before starting the tests. + if not arming:pre_arm_checks() then + return update_wait, UPDATE_PERIOD_MS + end + + -- Do some one time tests + local error_str = do_status_tests() + if error_str then + return complete(error_str) + end + + -- Continue on to the main list of tests. + return update_begin_test() +end + +local function _update_begin_test() + test_idx = test_idx + 1 + if test_idx > #test_data then + return complete() + end + + local dist_m_in = test_data[test_idx][1] + local signal_quality_pct_in = test_data[test_idx][2] + + -- Use the driver interface to simulate a data measurement being received and being passed to AP. + local result + if signal_quality_pct_in == -2 then + result = rngfnd_backend:handle_script_msg(dist_m_in) + else + result = rngfnd_backend:handle_script_msg(dist_m_in, signal_quality_pct_in) + end + if not result then + return complete(string.format("Test %i, dist_m: %.2f, quality_pct: %3i failed to handle_script_msg2", + test_idx, dist_m_in, signal_quality_pct_in)) + end + + return update_eval_test, UPDATE_PERIOD_MS +end + +local function _update_eval_test() + local dist_m_in = test_data[test_idx][1] + local signal_quality_pct_in = test_data[test_idx][2] + local status_expected = test_data[test_idx][3] + + -- Use the client interface to get distance data and ensure it matches the distance data + -- that was sent through the driver interface. + local error_str = get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_expected) + if error_str then + return complete(string.format("Test %i, dist_m: %.2f, quality_pct: %3i failed because %s", + test_idx, dist_m_in, signal_quality_pct_in, error_str)) + end + + -- Move to the next test in the list. + return update_begin_test() +end + +update_prepare = _update_prepare +update_wait = _update_wait +update_begin_test = _update_begin_test +update_eval_test = _update_eval_test + +send("Loaded rangefinder_quality_test.lua") + +return update_prepare, 0 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a37476a5252d93..4ad595570b6d74 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -200,16 +200,19 @@ include AP_RangeFinder/AP_RangeFinder_Backend.h ap_object AP_RangeFinder_Backend depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) ap_object AP_RangeFinder_Backend method distance float +ap_object AP_RangeFinder_Backend method signal_quality_pct float +ap_object AP_RangeFinder_Backend method signal_quality_pct rename signal_quality ap_object AP_RangeFinder_Backend method orientation Rotation'enum ap_object AP_RangeFinder_Backend method type uint8_t ap_object AP_RangeFinder_Backend method status uint8_t -ap_object AP_RangeFinder_Backend method handle_script_msg boolean float'skip_check +ap_object AP_RangeFinder_Backend manual handle_script_msg lua_range_finder_handle_script_msg 2 singleton RangeFinder depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) singleton RangeFinder rename rangefinder singleton RangeFinder method num_sensors uint8_t singleton RangeFinder method has_orientation boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method signal_quality_pct_orient int8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method max_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method min_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method ground_clearance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 1ff491ed740785..dd2571995c8934 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -771,4 +771,27 @@ int lua_print(lua_State *L) { return 0; } +#if (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) +int lua_range_finder_handle_script_msg(lua_State *L) { + + const int args = lua_gettop(L); + + // Arg 1 => self (an instance of rangefinder_backend) + // Arg 2 => a float distance + // Arg 3 => an optional float signal_quality + if (args != 2 and args != 3) { + return luaL_argerror(L, args, "expected 1 or 2 arguments"); + } + + AP_RangeFinder_Backend * ud = *check_AP_RangeFinder_Backend(L, 1); + const float distance_m = luaL_checknumber(L, 2); + const float signal_quality_pct = args == 3 ? luaL_checknumber(L, 3) : RangeFinder::SIGNAL_QUALITY_UNKNOWN; + + const bool result = ud->handle_script_msg(distance_m, signal_quality_pct); + lua_pushboolean(L, result); + + return 1; +} +#endif + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 8eef08bca35ac9..cf6653438efc21 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -20,3 +20,4 @@ int lua_mavlink_register_rx_msgid(lua_State *L); int lua_mavlink_send_chan(lua_State *L); int lua_mavlink_block_command(lua_State *L); int lua_print(lua_State *L); +int lua_range_finder_handle_script_msg(lua_State *L);