diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index dd5f68992a..a51e4b0f62 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/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 #include @@ -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); - break; #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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 497379fc47..15a794de12 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -102,6 +102,7 @@ public: USD1_CAN = 33, Benewake_CAN = 34, TeraRanger_Serial = 35, + Lua_Scripting = 36, SIM = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index e5e2a02d22..27694752e9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -34,6 +34,13 @@ public: 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp new file mode 100644 index 0000000000..b01842ef2c --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp @@ -0,0 +1,52 @@ +/* + 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 . + */ + +#include "AP_RangeFinder_Lua.h" +#include + +#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) +{ + 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h new file mode 100644 index 0000000000..e412a398b8 --- /dev/null +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 197305ebac..f49cf84c2f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/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),