AP_Scripting: Add lua interface to access range finder signal quality

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,

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
+

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 &params;

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

libraries/AP_RangeFinder/AP_RangeFinder_Lua.h

@@ -21,14 +21,15 @@ 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;
     }
 
 private:
 
-    float _distance_m;   // stored data from lua script:
+    RangeFinder::RangeFinder_State _state_pending = {};
 };
 
 #endif  // AP_RANGEFINDER_LUA_ENABLED

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