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

dataman: extend for double storage geofence and safe points

Browse Source
This commit is contained in:
Konrad
2024-01-30 11:25:37 -05:00
committed by Daniel Agar
parent dfa56d474a
commit 50f1abaef1
8 changed files with 79 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+15 -15
View File
@@ -108,7 +108,7 @@ void Geofence::run()
case DatamanState::Read:
_dataman_state = DatamanState::ReadWait;
success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&_stats),
sizeof(mission_stats_entry_s));
if (!success) {
@@ -139,8 +139,8 @@ void Geofence::run()
_dataman_cache.resize(_stats.num_items);
}
for (int index = 1; index <= _dataman_cache.size(); ++index) {
_dataman_cache.load(DM_KEY_FENCE_POINTS, index);
for (int index = 0; index < _dataman_cache.size(); ++index) {
_dataman_cache.load(DM_KEY_FENCE_POINTS_0, index);
}
_dataman_state = DatamanState::Load;
@@ -187,11 +187,11 @@ void Geofence::_updateFence()
// iterate over all polygons and store their starting vertices
_num_polygons = 0;
int current_seq = 1;
int current_seq = 0;
while (current_seq <= _dataman_cache.size()) {
while (current_seq < _dataman_cache.size()) {
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq,
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, current_seq,
reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
@@ -412,14 +412,14 @@ 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_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i,
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + i,
reinterpret_cast<uint8_t *>(&temp_vertex_i), sizeof(mission_fence_point_s));
if (!success) {
break;
}
success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j,
success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + j,
reinterpret_cast<uint8_t *>(&temp_vertex_j), sizeof(mission_fence_point_s));
if (!success) {
@@ -448,7 +448,7 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
{
mission_fence_point_s circle_point{};
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index,
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index,
reinterpret_cast<uint8_t *>(&circle_point), sizeof(mission_fence_point_s));
if (!success) {
@@ -554,7 +554,7 @@ Geofence::loadFromFile(const char *filename)
}
}
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast<uint8_t *>(&vertex),
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, pointCounter, reinterpret_cast<uint8_t *>(&vertex),
sizeof(vertex));
if (!success) {
@@ -585,16 +585,16 @@ Geofence::loadFromFile(const char *filename)
uint32_t crc32{0U};
/* do a second pass, now that we know the number of vertices */
for (int seq = 1; seq <= pointCounter; ++seq) {
for (int seq = 0; seq < pointCounter; ++seq) {
mission_fence_point_s mission_fence_point;
bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
if (success) {
mission_fence_point.vertex_count = pointCounter;
crc32 = crc32_for_fence_point(mission_fence_point, crc32);
_dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
_dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
}
}
@@ -603,7 +603,7 @@ Geofence::loadFromFile(const char *filename)
stats.num_items = pointCounter;
stats.opaque_id = crc32;
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (success) {
@@ -624,7 +624,7 @@ error:
int Geofence::clearDm()
{
_dataman_client.clearSync(DM_KEY_FENCE_POINTS);
_dataman_client.clearSync(DM_KEY_FENCE_POINTS_STATE);
updateFence();
return PX4_OK;
}
src/modules/navigator/navigation.h
+2 -2
View File
@@ -200,7 +200,7 @@ struct mission_item_s {
/**
* dataman housekeeping information for a specific item.
* Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS
* Corresponds to the dataman entry of DM_KEY_FENCE_POINTS_STATE and DM_KEY_SAFE_POINTS_STATE
*/
struct mission_stats_entry_s {
uint32_t opaque_id; /**< opaque identifier for current stored mission stats */
@@ -210,7 +210,7 @@ struct mission_stats_entry_s {
/**
* Geofence vertex point.
* Corresponds to the DM_KEY_FENCE_POINTS dataman item
* Corresponds to the DM_KEY_FENCE_POINTS_0 dataman item
*/
struct mission_fence_point_s {
double lat;
src/modules/navigator/rtl.cpp
+7 -7
View File
@@ -79,7 +79,7 @@ void RTL::updateDatamanCache()
case DatamanState::Read:
_dataman_state = DatamanState::ReadWait;
success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&_stats),
sizeof(mission_stats_entry_s));
if (!success) {
@@ -110,8 +110,8 @@ void RTL::updateDatamanCache()
_dataman_cache_safepoint.resize(_stats.num_items);
}
for (int index = 1; index <= _dataman_cache_safepoint.size(); ++index) {
_dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS, index);
for (int index = 0; index < _dataman_cache_safepoint.size(); ++index) {
_dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS_0, index);
}
_dataman_state = DatamanState::Load;
@@ -394,10 +394,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit
if (_safe_points_updated) {
for (int current_seq = 1; current_seq <= _dataman_cache_safepoint.size(); ++current_seq) {
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
mission_item_s mission_safe_point;
bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq,
bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS_0, current_seq,
reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_item_s), 500_ms);
@@ -626,10 +626,10 @@ land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position)
bool foundHomeLandApproaches = false;
uint8_t sector_counter = 0;
for (int current_seq = 1; current_seq <= _stats.num_items; ++current_seq) {
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_item{};
bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq,
bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS_0, current_seq,
reinterpret_cast<uint8_t *>(&mission_item),
sizeof(mission_item_s));