diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 40eb081cfb382..6d234c8ebfd66 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -166,6 +166,44 @@ def AltitudeHold(self): self.watch_altitude_maintained() self.disarm_vehicle() + def RngfndQuality(self): + """Check lua Range Finder quality information flow""" + self.context_push() + self.context_collect('STATUSTEXT') + + ex = None + try: + self.set_parameters({ + "SCR_ENABLE": 1, + "RNGFND1_TYPE": 36, + "RNGFND1_ORIENT": 25, + "RNGFND1_MIN_CM": 10, + "RNGFND1_MAX_CM": 5000, + }) + + self.install_example_script_context("rangefinder_quality_test.lua") + + # These string must match those sent by the lua test script. + complete_str = "#complete#" + failure_str = "!!failure!!" + + self.reboot_sitl() + + self.wait_statustext(complete_str, timeout=20, check_context=True) + found_failure = self.statustext_in_collections(failure_str) + + if found_failure is not None: + raise NotAchievedException("RngfndQuality test failed: " + found_failure.text) + + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.context_pop() + + if ex: + raise ex + def ModeChanges(self, delta=0.2): """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude""" self.wait_ready_to_arm() @@ -524,6 +562,7 @@ def tests(self): ret.extend([ self.DiveManual, self.AltitudeHold, + self.RngfndQuality, self.PositionHold, self.ModeChanges, self.DiveMission, diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 0809cd516a49a..bd0e9be2945ac 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -53,30 +53,39 @@ bool AP_RangeFinder_Backend::has_data() const { } // update status based on distance measurement -void AP_RangeFinder_Backend::update_status() +void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const { // check distance - if (state.distance_m > max_distance_cm() * 0.01f) { - set_status(RangeFinder::Status::OutOfRangeHigh); - } else if (state.distance_m < min_distance_cm() * 0.01f) { - set_status(RangeFinder::Status::OutOfRangeLow); + if (state_arg.distance_m > max_distance_cm() * 0.01f) { + set_status(state_arg, RangeFinder::Status::OutOfRangeHigh); + } else if (state_arg.distance_m < min_distance_cm() * 0.01f) { + set_status(state_arg, RangeFinder::Status::OutOfRangeLow); } else { - set_status(RangeFinder::Status::Good); + set_status(state_arg, RangeFinder::Status::Good); } } // set status and update valid count -void AP_RangeFinder_Backend::set_status(RangeFinder::Status _status) +void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status _status) { - state.status = _status; + state_arg.status = _status; // update valid count if (_status == RangeFinder::Status::Good) { - if (state.range_valid_count < 10) { - state.range_valid_count++; + if (state_arg.range_valid_count < 10) { + state_arg.range_valid_count++; } } else { - state.range_valid_count = 0; + state_arg.range_valid_count = 0; } } +#if AP_SCRIPTING_ENABLED +// get a copy of state structure +void AP_RangeFinder_Backend::get_state(RangeFinder::RangeFinder_State &state_arg) +{ + WITH_SEMAPHORE(_sem); + state_arg = state; +} +#endif + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 86f487364ba34..ca2decd044e30 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -36,9 +36,11 @@ class AP_RangeFinder_Backend virtual void handle_msg(const mavlink_message_t &msg) { return; } #if AP_SCRIPTING_ENABLED - // Returns false if scripting backing hasn't been setup - // Get distance from lua script - virtual bool handle_script_msg(float dist_m) { return false; } + void get_state(RangeFinder::RangeFinder_State &state_arg); + + // Returns false if scripting backing hasn't been setup. + virtual bool handle_script_msg(float dist_m) { return false; } // legacy interface + virtual bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) { return false; } #endif #if HAL_MSP_RANGEFINDER_ENABLED @@ -80,10 +82,12 @@ class AP_RangeFinder_Backend protected: // update status based on distance measurement - void update_status(); + void update_status(RangeFinder::RangeFinder_State &state_arg) const; + void update_status() { update_status(state); } // set status and update valid_count - void set_status(RangeFinder::Status status); + static void set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status status); + void set_status(RangeFinder::Status status) { set_status(state, status); } RangeFinder::RangeFinder_State &state; AP_RangeFinder_Params ¶ms; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp index cd024575136d2..75e22b2aaba5f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp @@ -24,31 +24,49 @@ AP_RangeFinder_Lua::AP_RangeFinder_Lua(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : AP_RangeFinder_Backend(_state, _params) { - set_status(RangeFinder::Status::NoData); } -// Set the distance based on a Lua Script -bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) +// Process range finder data from a lua driver. The state structure needs to be completely +// filled in by the lua script. The data passed to this method is copied to a pending_state +// structure. The update() method periodically copies data from pending_state to state. The get_state() +// method returns data from state. +bool AP_RangeFinder_Lua::handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) { - state.last_reading_ms = AP_HAL::millis(); - _distance_m = dist_m; + WITH_SEMAPHORE(_sem); + _state_pending = state_arg; return true; } +// Process range finder data from a lua driver - legacy interface. This method takes +// a distance measurement and fills in the pending state structure. In this legacy mode +// the lua script only passes in a distance measurement and does not manage the rest +// of the fields in the state structure. +bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) { -// update the state of the sensor -void AP_RangeFinder_Lua::update(void) -{ - //Time out on incoming data; if we don't get new - //data in 500ms, dump it - if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) { - set_status(RangeFinder::Status::NoData); - state.distance_m = 0.0f; + const uint32_t now = AP_HAL::millis(); + + WITH_SEMAPHORE(_sem); + + // Time out on incoming data; if we don't get new data in 500ms, dump it + if (now - _state_pending.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) { + set_status(_state_pending, RangeFinder::Status::NoData); } else { - state.distance_m = _distance_m; - update_status(); + _state_pending.last_reading_ms = now; + _state_pending.distance_m = dist_m; + _state_pending.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN; + _state_pending.voltage_mv = 0; + update_status(_state_pending); } + + return true; +} + +// Update the state of the sensor +void AP_RangeFinder_Lua::update(void) +{ + WITH_SEMAPHORE(_sem); + state = _state_pending; } #endif // AP_RANGEFINDER_LUA_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h index 12e5fad3675a2..0bd7f7a1acb6b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h @@ -21,6 +21,7 @@ class AP_RangeFinder_Lua : public AP_RangeFinder_Backend // Get update from Lua script bool handle_script_msg(float dist_m) override; + bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) override; MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_UNKNOWN; @@ -28,7 +29,7 @@ class AP_RangeFinder_Lua : public AP_RangeFinder_Backend private: - float _distance_m; // stored data from lua script: + RangeFinder::RangeFinder_State _state_pending = {}; }; #endif // AP_RANGEFINDER_LUA_ENABLED diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7e3be1f487dba..f11838f758f7f 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 0000000000000..28c78e7ff2515 --- /dev/null +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -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 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 94557841c84a0..62406fbf30f9c 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 a8637ed380547..eb884eb275cd8 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 99e4f8a374772..add40faa0a7bd 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);