Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Add LUA interface to access Range Finder state

This commit is contained in:
Peter Mullen 2023-11-25 21:03:55 -08:00 committed by Andrew Tridgell
parent 9cd85f2a2f
commit 2cc63f52a1
4 changed files with 64 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp
View File

@ -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

14
libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View File

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

48
libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp
View File

@ -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

3
libraries/AP_RangeFinder/AP_RangeFinder_Lua.h
View File

@ -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