dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

geofence: integrate dataman cache

This commit is contained in:
Igor Mišić 2023-05-10 13:20:59 +02:00 committed by Beat Küng
parent e14216c6c7
commit 4038eeec3e
3 changed files with 125 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
src/modules/navigator/geofence.cpp
View File

@ -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<uint8_t *>(&_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<uint8_t>(_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<uint8_t *>(&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<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq,
reinterpret_cast<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&temp_vertex_i), sizeof(mission_fence_point_s));
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i,
reinterpret_cast<uint8_t *>(&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<uint8_t *>(&temp_vertex_j), sizeof(mission_fence_point_s));
success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j,
reinterpret_cast<uint8_t *>(&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<uint8_t *>(&circle_point), sizeof(mission_fence_point_s));
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index,
reinterpret_cast<uint8_t *>(&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<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));

28
src/modules/navigator/geofence.h
View File

@ -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<vehicle_air_data_s> _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();

1
src/modules/navigator/navigator_main.cpp
View File

@ -918,6 +918,7 @@ void Navigator::run()
}
_mission.run();
_geofence.run();
perf_end(_loop_perf);
}