fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 20:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence: lock geofence items during a write transfers

Browse Source 
- avoids race conditions when geofence data is updated in flight. During
  a transfer, the geofence module will not check for violations, which
  is done with the new dm_trylock method.
- there is an update counter stored in dataman, and for each write
  transaction this is increased, so that the geofence module can reload
  the data upon data change (after it's unlocked).
- single dm item updates are atomic already, so resetting the polygons
  to 0 does not need locking.
This commit is contained in:
Beat Küng
2017-05-18 12:25:07 +02:00
committed by Lorenz Meier
parent cb580c5268
commit 6d85a3c4e4
5 changed files with 100 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+40 -1
View File
@@ -64,7 +64,9 @@ Geofence::Geofence(Navigator *navigator) :
_param_max_ver_distance(this, "GF_MAX_VER_DIST", false)
{
updateParams();
updateFence();
// we assume there's no concurrent fence update on startup
_updateFence();
}
Geofence::~Geofence()
@@ -76,6 +78,22 @@ Geofence::~Geofence()
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
int ret = dm_lock(DM_KEY_FENCE_POINTS);
if (ret != 0) {
PX4_ERR("lock failed");
return;
}
_updateFence();
dm_unlock(DM_KEY_FENCE_POINTS);
}
void Geofence::_updateFence()
{
// initialize fence points count
mission_stats_entry_s stats;
int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
@@ -83,6 +101,7 @@ void Geofence::updateFence()
if (ret == sizeof(mission_stats_entry_s)) {
num_fence_items = stats.num_items;
_update_counter = stats.update_counter;
}
// iterate over all polygons and store their starting vertices
@@ -268,7 +287,23 @@ bool Geofence::checkAll(double lat, double lon, float altitude)
bool Geofence::checkPolygons(double lat, double lon, float altitude)
{
// the following uses dm_read, 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
if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) {
return true;
}
// we got the lock, now check if the fence data got updated
mission_stats_entry_s stats;
int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) {
PX4_INFO("reloading geofence");
_updateFence();
}
if (isEmpty()) {
dm_unlock(DM_KEY_FENCE_POINTS);
/* Empty fence -> accept all points */
return true;
}
@@ -276,10 +311,12 @@ bool Geofence::checkPolygons(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) {
dm_unlock(DM_KEY_FENCE_POINTS);
return false;
}
}
/* Horizontal check: iterate all polygons & circles */
bool outside_exclusion = true;
bool inside_inclusion = false;
@@ -320,6 +357,8 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
}
}
dm_unlock(DM_KEY_FENCE_POINTS);
return (!had_inclusion_areas || inside_inclusion) && outside_exclusion;
}