From 4f01562c4a62cb121f04f1db5fb39d2ec170dbf0 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 state --- libraries/AP_Scripting/docs/docs.lua | 77 ++++- .../examples/rangefinder_quality_test.lua | 305 ++++++++++++++++++ .../generator/description/bindings.desc | 19 +- libraries/AP_Scripting/lua_bindings.cpp | 26 ++ libraries/AP_Scripting/lua_bindings.h | 1 + 5 files changed, 424 insertions(+), 4 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 7e3be1f487dba4..f11838f758f7f8 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2517,14 +2517,71 @@ function terrain:status() end ---@return boolean function terrain:enabled() end + +-- RangeFinder state structure +---@class RangeFinder_State_ud +local RangeFinder_State_ud = {} + +---@return RangeFinder_State_ud +function RangeFinder_State() end + +-- get system time (ms) of last successful update from sensor +---@return number +function RangeFinder_State_ud:last_reading() end + +-- set system time (ms) of last successful update from sensor +---@param value number +function RangeFinder_State_ud:last_reading(value) end + +-- get sensor status +---@return number +function RangeFinder_State_ud:status() end + +-- set sensor status +---@param value number +function RangeFinder_State_ud:status(value) end + +-- get number of consecutive valid readings (max out at 10) +---@return number +function RangeFinder_State_ud:range_valid_count() end + +-- set number of consecutive valid readings (max out at 10) +---@param value number +function RangeFinder_State_ud:range_valid_count(value) end + +-- get distance in meters +---@return number +function RangeFinder_State_ud:distance() end + +-- set distance in meters +---@param value number +function RangeFinder_State_ud:distance(value) end + +-- get measurement quality in percent 0-100, -1 -> quality is unknown +---@return number +function RangeFinder_State_ud:signal_quality() end + +-- set measurement quality in percent 0-100, -1 -> quality is unknown +---@param value number +function RangeFinder_State_ud:signal_quality(value) end + +-- get voltage in millivolts, if applicable, otherwise 0 +---@return number +function RangeFinder_State_ud:voltage() end + +-- set voltage in millivolts, if applicable, otherwise 0 +---@param value number +function RangeFinder_State_ud:voltage(value) end + + -- RangeFinder backend ---@class AP_RangeFinder_Backend_ud local AP_RangeFinder_Backend_ud = {} --- Send distance to lua rangefinder backend. Returns false if failed ----@param distance number +-- Send range finder measurement to lua rangefinder backend. Returns false if failed +---@param state RangeFinder_State_ud|number ---@return boolean -function AP_RangeFinder_Backend_ud:handle_script_msg(distance) end +function AP_RangeFinder_Backend_ud:handle_script_msg(state) end -- Status of this rangefinder instance ---@return integer @@ -2542,6 +2599,15 @@ 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 + +-- State of most recent range finder measurment +---@return RangeFinder_State_ud +function AP_RangeFinder_Backend_ud:get_state() end + + -- desc ---@class rangefinder rangefinder = {} @@ -2586,6 +2652,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..0c805f23cc007e --- /dev/null +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -0,0 +1,305 @@ + +-- 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 +-- Copied from libraries/AP_RangeFinder/AP_RangeFinder.h +local SIGNAL_QUALITY_MIN = 0 +local SIGNAL_QUALITY_MAX = 100 +local SIGNAL_QUALITY_UNKNOWN = -1 + +-- Read parameters for min and max valid range finder ranges. +local RNGFND1_MIN_CM = Parameter("RNGFND1_MIN_CM"):get() +local RNGFND1_MAX_CM = Parameter("RNGFND1_MAX_CM"):get() + +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 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("Status 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("Status test %i status correct - expected: %i actual: %i", test_idx, status_expected, status_actual)) + return nil + end + + -- L U A I N T E R F A C E T E S T + -- Check that the distance and signal_quality from the frontend are as expected + local distance1_cm_out = rangefinder:distance_cm_orient(RNGFND_ORIENTATION_DOWN) + local signal_quality1_pct_out = rangefinder:signal_quality_pct_orient(RNGFND_ORIENTATION_DOWN) + + -- Make sure data was returned + if not distance1_cm_out or not signal_quality1_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, distance1_cm_out, signal_quality_pct_in, signal_quality1_pct_out)) + + if not test_dist_equal(dist_m_in, 100.0, distance1_cm_out, signal_quality_pct_in, signal_quality1_pct_out) then + return "Frontend expected and actual do not match" + end + + -- L U A I N T E R F A C E T E S T + -- Check that the distance and signal_quality from the backend are as expected + local disttance2_m_out = rngfnd_backend:distance() + local signal_quality2_pct_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, disttance2_m_out, signal_quality_pct_in, signal_quality2_pct_out)) + + if not test_dist_equal(dist_m_in, 1.0, disttance2_m_out, signal_quality_pct_in, signal_quality2_pct_out) then + return "Backend expected and actual do not match" + end + + -- L U A I N T E R F A C E T E S T + -- Check that the state from the backend is as expected + local rf_state = rngfnd_backend:get_state() + local distance3_m_out = rf_state:distance() + local signal_quality3_pct_out = rf_state:signal_quality() + + send(string.format("State test %i dist in_m: %.2f out_m: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, distance3_m_out, signal_quality_pct_in, signal_quality3_pct_out)) + + if not test_dist_equal(dist_m_in, 1.0, distance3_m_out, signal_quality_pct_in, signal_quality3_pct_out) then + return "State 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 string.format("Status '%i' not NO_DATA on initialization.", status_actual) + 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, 127, RNDFND_STATUS_GOOD}, + {27.0, 3, RNDFND_STATUS_GOOD}, + {28.0, 100, RNDFND_STATUS_GOOD}, + {29.0, 99, RNDFND_STATUS_GOOD}, + {100.0, 100, RNDFND_STATUS_OUT_OF_RANGE_HIGH}, + {0.0, 100, RNDFND_STATUS_OUT_OF_RANGE_LOW}, + {100.0, -2, RNDFND_STATUS_OUT_OF_RANGE_HIGH}, + {0.0, -2, 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 + send(error_str) + status_str = FAILURE_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] + + -- L U A I N T E R F A C E T E S T + -- Use the driver interface to simulate a data measurement being received and being passed to AP. + local result + -- -2 => use legacy interface + if signal_quality_pct_in == -2 then + result = rngfnd_backend:handle_script_msg(dist_m_in) -- number as arg (compatibility mode) + + else + -- The full state udata must be initialized. + local rf_state = RangeFinder_State() + -- Set the status + if dist_m_in < RNGFND1_MIN_CM * 0.01 then + rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_LOW) + elseif dist_m_in > RNGFND1_MAX_CM * 0.01 then + rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_HIGH) + else + rf_state:status(RNDFND_STATUS_GOOD) + end + -- Sanitize signal_quality_pct_in + if signal_quality_pct_in < SIGNAL_QUALITY_MIN or signal_quality_pct_in > SIGNAL_QUALITY_MAX then + signal_quality_pct_in = SIGNAL_QUALITY_UNKNOWN + end + rf_state:last_reading(millis():toint()) + rf_state:range_valid_count(10) + rf_state:distance(dist_m_in) + rf_state:signal_quality(signal_quality_pct_in) + rf_state:voltage(0) + result = rngfnd_backend:handle_script_msg(rf_state) -- state as arg + 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 94557841c84a0e..62406fbf30f9c0 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -198,18 +198,35 @@ singleton AP_Proximity method get_backend AP_Proximity_Backend uint8_t'skip_chec include AP_RangeFinder/AP_RangeFinder.h include AP_RangeFinder/AP_RangeFinder_Backend.h +userdata RangeFinder::RangeFinder_State depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) +userdata RangeFinder::RangeFinder_State rename RangeFinder_State +userdata RangeFinder::RangeFinder_State field last_reading_ms uint32_t'skip_check read write +userdata RangeFinder::RangeFinder_State field last_reading_ms rename last_reading +userdata RangeFinder::RangeFinder_State field status RangeFinder::Status'enum read write RangeFinder::Status::NotConnected RangeFinder::Status::Good +userdata RangeFinder::RangeFinder_State field range_valid_count uint8_t'skip_check read write +userdata RangeFinder::RangeFinder_State field distance_m float'skip_check read write +userdata RangeFinder::RangeFinder_State field distance_m rename distance +userdata RangeFinder::RangeFinder_State field signal_quality_pct int8_t'skip_check read write +userdata RangeFinder::RangeFinder_State field signal_quality_pct rename signal_quality +userdata RangeFinder::RangeFinder_State field voltage_mv uint16_t'skip_check read write +userdata RangeFinder::RangeFinder_State field voltage_mv rename voltage + 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 1 +ap_object AP_RangeFinder_Backend method get_state void RangeFinder::RangeFinder_State'Ref 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 a8637ed380547a..eb884eb275cd87 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -950,4 +950,30 @@ 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) { + // Arg 1 => self (an instance of rangefinder_backend) + // Arg 2 => a float distance or a RangeFinder_State user data + binding_argcheck(L, 2); + + // check_AP_RangeFinder_Backend aborts if not found. No need to check for null + AP_RangeFinder_Backend * ud = *check_AP_RangeFinder_Backend(L, 1); + + bool result = false; + + // Check to see if the first argument is the state structure. + const void *state_arg = luaL_testudata(L, 2, "RangeFinder_State"); + if (state_arg != nullptr) { + result = ud->handle_script_msg(*static_cast(state_arg)); + + } else { + // Otherwise assume the argument is a number and set the measurement. + result = ud->handle_script_msg(luaL_checknumber(L, 2)); + } + + 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 99e4f8a3747727..add40faa0a7bd8 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -26,3 +26,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);