AP_Scripting: Add LUA interface to access Range Finder signal_quality…

… data

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

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

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

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

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);