AP_Scripting: Add LUA interface to access Range Finder state

This commit is contained in:
Peter Mullen 2023-11-25 21:05:45 -08:00 committed by Andrew Tridgell
parent 948ee94cee
commit a83ed6b730
5 changed files with 423 additions and 4 deletions

View File

@ -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

View File

@ -0,0 +1,304 @@
-- 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_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_NOT_CONNECTED then
return string.format("DOWN Status '%i' not NOT_CONNECTED on initialization.", status_actual)
end
status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_FORWARD)
if status_actual ~= RNDFND_STATUS_NOT_CONNECTED then
return string.format("FORWARD Status '%i' not NOT_CONNECTED on initialization.", status_actual)
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

View File

@ -200,18 +200,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

View File

@ -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<const RangeFinder::RangeFinder_State *>(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

View File

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