From 2cc63f52a1d61424584f06c504374e33c8d2c9a0 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sat, 25 Nov 2023 21:03:55 -0800 Subject: [PATCH] AP_RangeFinder: Add LUA interface to access Range Finder state --- .../AP_RangeFinder/AP_RangeFinder_Backend.cpp | 31 +++++++----- .../AP_RangeFinder/AP_RangeFinder_Backend.h | 14 ++++-- .../AP_RangeFinder/AP_RangeFinder_Lua.cpp | 48 +++++++++++++------ libraries/AP_RangeFinder/AP_RangeFinder_Lua.h | 3 +- 4 files changed, 64 insertions(+), 32 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 0809cd516a..bd0e9be294 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 86f487364b..ca2decd044 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -36,9 +36,11 @@ public: 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 @@ public: 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 cd02457513..75e22b2aab 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 + 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_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) { - //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(); - } + 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 12e5fad367..0bd7f7a1ac 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h @@ -21,6 +21,7 @@ public: // 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 @@ public: private: - float _distance_m; // stored data from lua script: + RangeFinder::RangeFinder_State _state_pending = {}; }; #endif // AP_RANGEFINDER_LUA_ENABLED