From 4038eeec3e97153c3b12d22d51950c8335dc2271 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Wed, 10 May 2023 13:20:59 +0200 Subject: [PATCH] geofence: integrate dataman cache --- src/modules/navigator/geofence.cpp | 160 ++++++++++++++--------- src/modules/navigator/geofence.h | 28 +++- src/modules/navigator/navigator_main.cpp | 1 + 3 files changed, 125 insertions(+), 64 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 10c9bf7ddc..f6de5654f1 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,9 +58,8 @@ Geofence::Geofence(Navigator *navigator) : _navigator(navigator), _sub_airdata(ORB_ID(vehicle_air_data)) { - // we assume there's no concurrent fence update on startup if (_navigator != nullptr) { - _updateFence(); + updateFence(); } } @@ -71,47 +70,113 @@ Geofence::~Geofence() } } +void Geofence::run() +{ + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_fence_updated) { + _initiate_fence_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client.update(); + + if (_dataman_client.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _fence_updated = false; + + _dataman_cache.invalidate(); + + if (_dataman_cache.size() != _stats.num_items) { + _dataman_cache.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache.size(); ++index) { + _dataman_cache.load(DM_KEY_FENCE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache.update(); + + if (!_dataman_cache.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _updateFence(); + _fence_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Geofence update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + + } +} + void Geofence::updateFence() { - // Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer. - // However this is currently not used - bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS); - - if (!success) { - PX4_ERR("lock failed"); - return; - } - - _updateFence(); - _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); + _initiate_fence_updated = true; } void Geofence::_updateFence() { - // initialize fence points count - mission_stats_entry_s stats; - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), - sizeof(mission_stats_entry_s)); - int num_fence_items = 0; - - if (success) { - num_fence_items = stats.num_items; - _update_counter = stats.update_counter; - } + mission_fence_point_s mission_fence_point; + bool is_circle_area = false; // iterate over all polygons and store their starting vertices _num_polygons = 0; int current_seq = 1; - while (current_seq <= num_fence_items) { - mission_fence_point_s mission_fence_point; - bool is_circle_area = false; + while (current_seq <= _dataman_cache.size()) { - success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, current_seq, reinterpret_cast(&mission_fence_point), - sizeof(mission_fence_point_s)); + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq, + reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); if (!success) { - PX4_ERR("dm_read failed"); + PX4_ERR("loadWait failed, seq: %i", current_seq); break; } @@ -177,9 +242,7 @@ void Geofence::_updateFence() ++current_seq; break; } - } - } bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) @@ -311,25 +374,7 @@ bool Geofence::isBelowMaxAltitude(float altitude) bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { - // the following uses readSync, so first we try to lock all items. If that fails, it (most likely) means - // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now - bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS); - - if (!success) { - return true; - } - - // we got the lock, now check if the fence data got updated - mission_stats_entry_s stats; - success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), - sizeof(mission_stats_entry_s)); - - if (success && _update_counter != stats.update_counter) { - _updateFence(); - } - if (isEmpty()) { - _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); /* Empty fence -> accept all points */ return true; } @@ -337,12 +382,10 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) /* Vertical check */ if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly if (altitude > _altitude_max || altitude < _altitude_min) { - _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); return false; } } - /* Horizontal check: iterate all polygons & circles */ bool outside_exclusion = true; bool inside_inclusion = false; @@ -383,8 +426,6 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) } } - _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); - return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; } @@ -403,15 +444,15 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, - reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, + reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); if (!success) { break; } - success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, - reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); + success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, + reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); if (!success) { break; @@ -439,8 +480,8 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, { mission_fence_point_s circle_point{}; - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index, - reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index, + reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); if (!success) { PX4_ERR("dm_read failed"); @@ -590,6 +631,7 @@ Geofence::loadFromFile(const char *filename) mission_stats_entry_s stats; stats.num_items = pointCounter; + stats.update_counter = _update_counter + 1; bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index ffcb800cb2..1804d151d5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -78,9 +78,13 @@ public: GF_SOURCE_GPS = 1 }; + /** + * @brief function to call regularly to do background work + */ + void run(); + /** * update the geofence from dataman. - * It's generally not necessary to call this as it will automatically update when the data is changed. */ void updateFence(); @@ -137,7 +141,7 @@ public: */ int loadFromFile(const char *filename); - bool isEmpty() { return _num_polygons == 0; } + bool isEmpty() { return (!_fence_updated || (_num_polygons == 0)); } int getSource() { return _param_gf_source.get(); } int getGeofenceAction() { return _param_gf_action.get(); } @@ -155,6 +159,14 @@ public: private: + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error + }; + struct PolygonInfo { uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region) uint16_t dataman_index; @@ -167,7 +179,11 @@ private: Navigator *_navigator{nullptr}; PolygonInfo *_polygons{nullptr}; - DatamanClient _dataman_client{}; + mission_stats_entry_s _stats; + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; + DatamanClient &_dataman_client = _dataman_cache.client(); hrt_abstime _last_horizontal_range_warning{0}; hrt_abstime _last_vertical_range_warning{0}; @@ -182,10 +198,12 @@ private: uORB::SubscriptionData _sub_airdata; int _outside_counter{0}; - uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, polygon data was updated + bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache + bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed /** - * implementation of updateFence(), but without locking + * implementation of updateFence() */ void _updateFence(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3353bb4806..704f125dd5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -918,6 +918,7 @@ void Navigator::run() } _mission.run(); + _geofence.run(); perf_end(_loop_perf); }