Scripting/AP_RangeFinder: Add lua scripting based RangeFinder

libraries/AP_DAL/AP_DAL_RangeFinder.cpp

@@ -99,6 +99,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
     _RRNI.status = (uint8_t)backend->status();
     _RRNI.pos_offset = backend->get_pos_offset();
     _RRNI.distance_cm = backend->distance_cm();
+    _RRNI.use = backend->use_sensor();
     WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
 }
 

libraries/AP_DAL/AP_DAL_RangeFinder.h

@@ -61,6 +61,8 @@ class AP_DAL_RangeFinder_Backend {
 
     const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; }
 
+    bool use_sensor() const { return _RRNI.use; }
+
     // DAL methods:
     void start_frame(AP_RangeFinder_Backend *backend);
 

libraries/AP_DAL/LogStructure.h

@@ -172,6 +172,7 @@ struct log_RRNI {
     uint8_t orientation;
     uint8_t status;
     uint8_t instance;
+    bool use;
     uint8_t _end;
 };
 
@@ -396,7 +397,7 @@ struct log_RBOH {
     { LOG_RRNH_MSG, RLOG_SIZE(RRNH),                                   \
       "RRNH", "hhB", "GCl,MaxD,NumSensors", "???", "???" },  \
     { LOG_RRNI_MSG, RLOG_SIZE(RRNI),                                   \
-      "RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \
+      "RRNI", "fffHBBBB", "PX,PY,PZ,Dist,Orient,Status,I,Use", "-------#", "--------" }, \
     { LOG_RGPH_MSG, RLOG_SIZE(RGPH),                                   \
       "RGPH", "BB", "NumInst,Primary", "--", "--" },  \
     { LOG_RGPI_MSG, RLOG_SIZE(RGPI),                                   \

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -44,7 +44,7 @@ void NavEKF2_core::readRangeFinder(void)
             if (sensor == nullptr) {
                 continue;
             }
-            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
+            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good) && sensor->use_sensor()) {
                 rngMeasIndex[sensorIndex] ++;
                 if (rngMeasIndex[sensorIndex] > 2) {
                     rngMeasIndex[sensorIndex] = 0;

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -39,7 +39,7 @@ void NavEKF3_core::readRangeFinder(void)
             if (sensor == nullptr) {
                 continue;
             }
-            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
+            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good) && sensor->use_sensor()) {
                 rngMeasIndex[sensorIndex] ++;
                 if (rngMeasIndex[sensorIndex] > 2) {
                     rngMeasIndex[sensorIndex] = 0;

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -40,7 +40,7 @@ void AP_Proximity_RangeFinder::update(void)
         if (sensor == nullptr) {
             continue;
         }
-        if (sensor->has_data()) {
+        if (sensor->has_data() && sensor->use_sensor()) {
             // check for horizontal range finders
             if (sensor->orientation() <= ROTATION_YAW_315) {
                 const uint8_t sector = (uint8_t)sensor->orientation();

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -51,6 +51,7 @@
 #include "AP_RangeFinder_MSP.h"
 #include "AP_RangeFinder_USD1_CAN.h"
 #include "AP_RangeFinder_Benewake_CAN.h"
+#include "AP_RangeFinder_Lua.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -159,7 +160,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp
     AP_SUBGROUPVARPTR(drivers[9], "A_",  44, RangeFinder, backend_var_info[9]),
 #endif
-    
+
     AP_GROUPEND
 };
 
@@ -524,8 +525,15 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::Benewake_CAN:
 #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
         _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
+#endif
         break;
+
+    case Type::Lua_Scripting:
+#if AP_SCRIPTING_ENABLED
+        _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance);
 #endif
+        break;
+
     case Type::NONE:
         break;
     }
@@ -603,12 +611,13 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const
 // find first range finder instance with the specified orientation
 AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
 {
-    // first try for a rangefinder that is in range
+    // first try for a rangefinder that is in range and also in use
     for (uint8_t i=0; i<num_instances; i++) {
         AP_RangeFinder_Backend *backend = get_backend(i);
         if (backend != nullptr &&
             backend->orientation() == orientation &&
-            backend->status() == Status::Good) {
+            backend->status() == Status::Good &&
+            backend->use_sensor()) {
             return backend;
         }
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -102,6 +102,7 @@ class RangeFinder
         USD1_CAN = 33,
         Benewake_CAN = 34,
         TeraRanger_Serial = 35,
+        Lua_Scripting = 36,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -34,6 +34,13 @@ class AP_RangeFinder_Backend
     virtual void init_serial(uint8_t serial_instance) {};
 
     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; }
+#endif
+
 #if HAL_MSP_RANGEFINDER_ENABLED
     virtual void handle_msp(const MSP::msp_rangefinder_data_message_t &pkt) { return; }
 #endif
@@ -48,6 +55,7 @@ class AP_RangeFinder_Backend
     MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
     RangeFinder::Status status() const;
     RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); }
+    bool use_sensor() const { return params.use; }
 
     // true if sensor is returning data
     bool has_data() const;

libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp

@@ -0,0 +1,53 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_RangeFinder_Lua.h"
+#include <AP_HAL/AP_HAL.h>
+
+#if AP_SCRIPTING_ENABLED
+
+// constructor
+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)
+{
+    // only accept distances for the configured orentation
+    state.last_reading_ms = AP_HAL::millis();
+    _distance_m = dist_m;
+    return true;
+}
+
+
+// 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;
+    } else {
+        state.distance_m = _distance_m;
+        update_status();
+    }
+}
+
+#endif

libraries/AP_RangeFinder/AP_RangeFinder_Lua.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend.h"
+
+#if AP_SCRIPTING_ENABLED
+
+// Data timeout
+#define AP_RANGEFINDER_LUA_TIMEOUT_MS 500
+
+class AP_RangeFinder_Lua : public AP_RangeFinder_Backend
+{
+public:
+
+    // constructor
+    AP_RangeFinder_Lua(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+
+    // update state
+    void update(void) override;
+
+    // Get update from Lua script
+    bool handle_script_msg(float dist_m) 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:
+};
+
+#endif

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -15,7 +15,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,100:SITL
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 
@@ -138,6 +138,13 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ORIENT", 53, AP_RangeFinder_Params, orientation, AP_RANGEFINDER_DEFAULT_ORIENTATION),
 
+    // @Param: USE
+    // @DisplayName: Rangefinder to use
+    // @Description: Setting this parameter to 0 will cause the RangeFinder to be used for Logging/Lua Scripting only.
+    // @Values: 0:Do not use sensor, 1: Use sensor
+    // @User: Advanced
+    AP_GROUPINFO("USE", 54, AP_RangeFinder_Params, use, 1),
+
     AP_GROUPEND
 };
 

libraries/AP_RangeFinder/AP_RangeFinder_Params.h

@@ -26,4 +26,5 @@ class AP_RangeFinder_Params {
     AP_Int8  ground_clearance_cm;
     AP_Int8  address;
     AP_Int8  orientation;
+    AP_Int8  use;
 };

libraries/AP_Scripting/docs/docs.lua

@@ -1870,11 +1870,44 @@ function terrain:status() end
 ---@return boolean
 function terrain:enabled() 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
+---@return boolean
+function AP_RangeFinder_Backend_ud:handle_script_msg(distance) end
+
+-- Status of this rangefinder instance
+---@return integer
+function AP_RangeFinder_Backend_ud:status() end
+
+-- Type of rangefinder of this instance
+---@return integer
+function AP_RangeFinder_Backend_ud:type() end
+
+-- Orintation of the rangefinder of this instance
+---@return integer
+function AP_RangeFinder_Backend_ud:orientation() end
+
+-- Current distance of the sensor instance
+---@return number
+function AP_RangeFinder_Backend_ud:distance() end
+
+-- Returns true if backend has new data
+---@return boolean
+function AP_RangeFinder_Backend_ud:has_data() end
 
 -- desc
 ---@class rangefinder
 rangefinder = {}
 
+-- get backend based on rangefinder instance provided
+---@param rangefinder_instance integer
+---@return AP_RangeFinder_Backend_ud
+function rangefinder:get_backend(rangefinder_instance) end
+
 -- desc
 ---@param orientation integer
 ---@return Vector3f_ud