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

dataman: extend for double storage geofence and safe points

This commit is contained in:
Konrad 2023-12-04 14:34:23 +01:00 committed by Daniel Agar
parent dfa56d474a
commit 50f1abaef1
8 changed files with 79 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/dataman/dataman.cpp
View File

@ -129,8 +129,12 @@ static unsigned g_func_counts[DM_NUMBER_OF_FUNCS];
/* Table of the len of each item type including HDR size */ /* Table of the len of each item type including HDR size */
static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = { static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = {
g_per_item_size[DM_KEY_SAFE_POINTS] + DM_SECTOR_HDR_SIZE, g_per_item_size[DM_KEY_SAFE_POINTS_0] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_FENCE_POINTS] + DM_SECTOR_HDR_SIZE, g_per_item_size[DM_KEY_SAFE_POINTS_1] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_SAFE_POINTS_STATE] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_FENCE_POINTS_0] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_FENCE_POINTS_1] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_FENCE_POINTS_STATE] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0] + DM_SECTOR_HDR_SIZE, g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_1] + DM_SECTOR_HDR_SIZE, g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_1] + DM_SECTOR_HDR_SIZE,
g_per_item_size[DM_KEY_MISSION_STATE] + DM_SECTOR_HDR_SIZE, g_per_item_size[DM_KEY_MISSION_STATE] + DM_SECTOR_HDR_SIZE,
@ -564,7 +568,7 @@ _file_initialize(unsigned max_offset)
PX4_ERR("Failed writing compat: %d", ret); PX4_ERR("Failed writing compat: %d", ret);
} }
for (uint32_t item = DM_KEY_SAFE_POINTS; item <= DM_KEY_MISSION_STATE; ++item) { for (uint32_t item = DM_KEY_SAFE_POINTS_0; item <= DM_KEY_MISSION_STATE; ++item) {
g_dm_ops->clear((dm_item_t)item); g_dm_ops->clear((dm_item_t)item);
} }
@ -582,8 +586,8 @@ _file_initialize(unsigned max_offset)
stats.opaque_id = 0; stats.opaque_id = 0;
g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s)); g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
g_dm_ops->write(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), sizeof(mission_stats_entry_s)); g_dm_ops->write(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats), sizeof(mission_stats_entry_s));
g_dm_ops->write(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), sizeof(mission_stats_entry_s)); g_dm_ops->write(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats), sizeof(mission_stats_entry_s));
} }
dm_operations_data.running = true; dm_operations_data.running = true;
@ -877,10 +881,6 @@ Each type has a specific type and a fixed maximum amount of storage items, so th
### Implementation ### Implementation
Reading and writing a single item is always atomic. Reading and writing a single item is always atomic.
**DM_KEY_FENCE_POINTS** and **DM_KEY_SAFE_POINTS** items: the first data element is a `mission_stats_entry_s` struct,
which stores the number of items for these types. These items are always updated atomically in one transaction (from
the mavlink mission manager).
)DESCR_STR"); )DESCR_STR");
PRINT_MODULE_USAGE_NAME("dataman", "system"); PRINT_MODULE_USAGE_NAME("dataman", "system");

24
src/modules/dataman/dataman.h
View File

@ -44,8 +44,12 @@
/** Types of items that the data manager can store */ /** Types of items that the data manager can store */
typedef enum { typedef enum {
DM_KEY_SAFE_POINTS = 0, ///< Safe points coordinates, safe point 0 is home point DM_KEY_SAFE_POINTS_0 = 0, ///< Safe points storage 0
DM_KEY_FENCE_POINTS, ///< Fence vertex coordinates DM_KEY_SAFE_POINTS_1, ///< Safe points storage 1 (alternate between 0 and 1)
DM_KEY_SAFE_POINTS_STATE, ///< Persistent safe point state
DM_KEY_FENCE_POINTS_0, ///< Fence vertex storage 0
DM_KEY_FENCE_POINTS_1, ///< Fence vertex storage 1 (alternate between 0 and 1)
DM_KEY_FENCE_POINTS_STATE, ///< Persistent fence vertex state
DM_KEY_WAYPOINTS_OFFBOARD_0, ///< Mission way point coordinates sent over mavlink DM_KEY_WAYPOINTS_OFFBOARD_0, ///< Mission way point coordinates sent over mavlink
DM_KEY_WAYPOINTS_OFFBOARD_1, ///< (alternate between 0 and 1) DM_KEY_WAYPOINTS_OFFBOARD_1, ///< (alternate between 0 and 1)
DM_KEY_MISSION_STATE, ///< Persistent mission state DM_KEY_MISSION_STATE, ///< Persistent mission state
@ -66,7 +70,9 @@ typedef enum {
#if defined(MEMORY_CONSTRAINED_SYSTEM) #if defined(MEMORY_CONSTRAINED_SYSTEM)
enum { enum {
DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_SAFE_POINTS_STATE_MAX = 1,
DM_KEY_FENCE_POINTS_MAX = 16, DM_KEY_FENCE_POINTS_MAX = 16,
DM_KEY_FENCE_POINTS_STATE_MAX = 1,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_MISSION_STATE_MAX = 1, DM_KEY_MISSION_STATE_MAX = 1,
@ -75,7 +81,9 @@ enum {
#else #else
enum { enum {
DM_KEY_SAFE_POINTS_MAX = 32, DM_KEY_SAFE_POINTS_MAX = 32,
DM_KEY_SAFE_POINTS_STATE_MAX = 1,
DM_KEY_FENCE_POINTS_MAX = 64, DM_KEY_FENCE_POINTS_MAX = 64,
DM_KEY_FENCE_POINTS_STATE_MAX = 1,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_MISSION_STATE_MAX = 1, DM_KEY_MISSION_STATE_MAX = 1,
@ -86,7 +94,11 @@ enum {
/* table of maximum number of instances for each item type */ /* table of maximum number of instances for each item type */
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_SAFE_POINTS_MAX, DM_KEY_SAFE_POINTS_MAX,
DM_KEY_SAFE_POINTS_MAX,
DM_KEY_SAFE_POINTS_STATE_MAX,
DM_KEY_FENCE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX,
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_FENCE_POINTS_STATE_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
DM_KEY_MISSION_STATE_MAX, DM_KEY_MISSION_STATE_MAX,
@ -98,7 +110,9 @@ struct dataman_compat_s {
}; };
constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_item_s); constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_item_s);
constexpr uint32_t MISSION_SAFE_POINT_STATE_SIZE = sizeof(struct mission_stats_entry_s);
constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s); constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s);
constexpr uint32_t MISSION_FENCE_POINT_STATE_SIZE = sizeof(struct mission_stats_entry_s);
constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s); constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s);
constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s); constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s);
constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct dataman_compat_s); constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct dataman_compat_s);
@ -106,7 +120,11 @@ constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct dataman_compat_s);
/** The table of the size of each item type */ /** The table of the size of each item type */
static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
MISSION_SAFE_POINT_SIZE, MISSION_SAFE_POINT_SIZE,
MISSION_SAFE_POINT_SIZE,
MISSION_SAFE_POINT_STATE_SIZE,
MISSION_FENCE_POINT_SIZE, MISSION_FENCE_POINT_SIZE,
MISSION_FENCE_POINT_SIZE,
MISSION_FENCE_POINT_STATE_SIZE,
MISSION_ITEM_SIZE, MISSION_ITEM_SIZE,
MISSION_ITEM_SIZE, MISSION_ITEM_SIZE,
MISSION_SIZE, MISSION_SIZE,
@ -114,7 +132,7 @@ static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
}; };
/* increment this define whenever a binary incompatible change is performed */ /* increment this define whenever a binary incompatible change is performed */
#define DM_COMPAT_VERSION 4ULL #define DM_COMPAT_VERSION 5ULL
#define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \ #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \
(sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \ (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \

16
src/modules/mavlink/mavlink_mission.cpp
View File

@ -110,7 +110,7 @@ MavlinkMissionManager::load_geofence_stats()
{ {
mission_stats_entry_s stats; mission_stats_entry_s stats;
// initialize fence points count // initialize fence points count
bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s)); sizeof(mission_stats_entry_s));
if (success) { if (success) {
@ -126,7 +126,7 @@ MavlinkMissionManager::load_safepoint_stats()
{ {
mission_stats_entry_s stats; mission_stats_entry_s stats;
// initialize safe points count // initialize safe points count
bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s)); sizeof(mission_stats_entry_s));
if (success) { if (success) {
@ -182,7 +182,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
stats.opaque_id = crc32; stats.opaque_id = crc32;
/* update stats in dataman */ /* update stats in dataman */
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)); sizeof(mission_stats_entry_s));
if (success) { if (success) {
@ -214,7 +214,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
stats.opaque_id = crc32; stats.opaque_id = crc32;
/* update stats in dataman */ /* update stats in dataman */
bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s)); sizeof(mission_stats_entry_s));
if (success) { if (success) {
@ -303,7 +303,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
case MAV_MISSION_TYPE_FENCE: { // Read a geofence point case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
mission_fence_point_s mission_fence_point; mission_fence_point_s mission_fence_point;
read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1, read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_STATE, seq,
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s)); reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
mission_item.nav_cmd = mission_fence_point.nav_cmd; mission_item.nav_cmd = mission_fence_point.nav_cmd;
@ -323,7 +323,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
break; break;
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast<uint8_t *>(&mission_item), read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_item),
sizeof(mission_item_s)); sizeof(mission_item_s));
} }
break; break;
@ -1152,7 +1152,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
mission_fence_point.frame = mission_item.frame; mission_fence_point.frame = mission_item.frame;
if (!check_failed) { if (!check_failed) {
write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1, write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, wp.seq,
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s)); reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
} }
@ -1160,7 +1160,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
break; break;
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1, write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS_0, wp.seq,
reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s); reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s);
} }
break; break;

4
src/modules/mavlink/mavlink_mission.h
View File

@ -148,8 +148,8 @@ private:
2; ///< Error count limit before stopping to report FS errors 2; ///< Error count limit before stopping to report FS errors
static constexpr uint16_t MAX_COUNT[] = { static constexpr uint16_t MAX_COUNT[] = {
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_FENCE_POINTS_MAX - 1, DM_KEY_FENCE_POINTS_MAX,
DM_KEY_SAFE_POINTS_MAX - 1 DM_KEY_SAFE_POINTS_MAX
}; /**< Maximum number of mission items for each type }; /**< Maximum number of mission items for each type
(fence & safe points use the first item for the stats) */ (fence & safe points use the first item for the stats) */

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

@ -108,7 +108,7 @@ void Geofence::run()
case DatamanState::Read: case DatamanState::Read:
_dataman_state = DatamanState::ReadWait; _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)); sizeof(mission_stats_entry_s));
if (!success) { if (!success) {
@ -139,8 +139,8 @@ void Geofence::run()
_dataman_cache.resize(_stats.num_items); _dataman_cache.resize(_stats.num_items);
} }
for (int index = 1; index <= _dataman_cache.size(); ++index) { for (int index = 0; index < _dataman_cache.size(); ++index) {
_dataman_cache.load(DM_KEY_FENCE_POINTS, index); _dataman_cache.load(DM_KEY_FENCE_POINTS_0, index);
} }
_dataman_state = DatamanState::Load; _dataman_state = DatamanState::Load;
@ -187,11 +187,11 @@ void Geofence::_updateFence()
// iterate over all polygons and store their starting vertices // iterate over all polygons and store their starting vertices
_num_polygons = 0; _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), reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s)); 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++) { 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)); reinterpret_cast<uint8_t *>(&temp_vertex_i), sizeof(mission_fence_point_s));
if (!success) { if (!success) {
break; 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)); reinterpret_cast<uint8_t *>(&temp_vertex_j), sizeof(mission_fence_point_s));
if (!success) { if (!success) {
@ -448,7 +448,7 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
{ {
mission_fence_point_s circle_point{}; 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)); reinterpret_cast<uint8_t *>(&circle_point), sizeof(mission_fence_point_s));
if (!success) { 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)); sizeof(vertex));
if (!success) { if (!success) {
@ -585,16 +585,16 @@ Geofence::loadFromFile(const char *filename)
uint32_t crc32{0U}; uint32_t crc32{0U};
/* do a second pass, now that we know the number of vertices */ /* 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; 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)); sizeof(mission_fence_point_s));
if (success) { if (success) {
mission_fence_point.vertex_count = pointCounter; mission_fence_point.vertex_count = pointCounter;
crc32 = crc32_for_fence_point(mission_fence_point, crc32); 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)); sizeof(mission_fence_point_s));
} }
} }
@ -603,7 +603,7 @@ Geofence::loadFromFile(const char *filename)
stats.num_items = pointCounter; stats.num_items = pointCounter;
stats.opaque_id = crc32; 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)); sizeof(mission_stats_entry_s));
if (success) { if (success) {
@ -624,7 +624,7 @@ error:
int Geofence::clearDm() int Geofence::clearDm()
{ {
_dataman_client.clearSync(DM_KEY_FENCE_POINTS); _dataman_client.clearSync(DM_KEY_FENCE_POINTS_STATE);
updateFence(); updateFence();
return PX4_OK; return PX4_OK;
} }

4
src/modules/navigator/navigation.h
View File

@ -200,7 +200,7 @@ struct mission_item_s {
/** /**
* dataman housekeeping information for a specific item. * 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 { struct mission_stats_entry_s {
uint32_t opaque_id; /**< opaque identifier for current stored mission stats */ uint32_t opaque_id; /**< opaque identifier for current stored mission stats */
@ -210,7 +210,7 @@ struct mission_stats_entry_s {
/** /**
* Geofence vertex point. * 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 { struct mission_fence_point_s {
double lat; double lat;

14
src/modules/navigator/rtl.cpp
View File

@ -79,7 +79,7 @@ void RTL::updateDatamanCache()
case DatamanState::Read: case DatamanState::Read:
_dataman_state = DatamanState::ReadWait; _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)); sizeof(mission_stats_entry_s));
if (!success) { if (!success) {
@ -110,8 +110,8 @@ void RTL::updateDatamanCache()
_dataman_cache_safepoint.resize(_stats.num_items); _dataman_cache_safepoint.resize(_stats.num_items);
} }
for (int index = 1; index <= _dataman_cache_safepoint.size(); ++index) { for (int index = 0; index < _dataman_cache_safepoint.size(); ++index) {
_dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS, index); _dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS_0, index);
} }
_dataman_state = DatamanState::Load; _dataman_state = DatamanState::Load;
@ -394,10 +394,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit
if (_safe_points_updated) { 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; 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), reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_item_s), 500_ms); sizeof(mission_item_s), 500_ms);
@ -626,10 +626,10 @@ land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position)
bool foundHomeLandApproaches = false; bool foundHomeLandApproaches = false;
uint8_t sector_counter = 0; 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{}; 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), reinterpret_cast<uint8_t *>(&mission_item),
sizeof(mission_item_s)); sizeof(mission_item_s));

29
src/systemcmds/tests/test_dataman.cpp
View File

@ -153,14 +153,14 @@ DatamanTest::testSyncWriteInvalidItem()
bool bool
DatamanTest::testSyncReadInvalidIndex() DatamanTest::testSyncReadInvalidIndex()
{ {
bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2);
return !success; return !success;
} }
bool bool
DatamanTest::testSyncWriteInvalidIndex() DatamanTest::testSyncWriteInvalidIndex()
{ {
bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2);
return !success; return !success;
} }
@ -238,7 +238,7 @@ DatamanTest::testSyncWriteReadAllItemsMaxSize()
bool success = false; bool success = false;
// Iterate all items // Iterate all items
for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { for (uint32_t item = DM_KEY_SAFE_POINTS_0; item < DM_KEY_NUM_KEYS; ++item) {
// writeSync // writeSync
for (uint32_t index = 0U; index < _max_index[item]; ++index) { for (uint32_t index = 0U; index < _max_index[item]; ++index) {
@ -289,7 +289,7 @@ DatamanTest::testSyncClearAll()
bool success = false; bool success = false;
// Iterate all items // Iterate all items
for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { for (uint32_t item = DM_KEY_SAFE_POINTS_0; item < DM_KEY_NUM_KEYS; ++item) {
success = _dataman_client1.clearSync((dm_item_t)item); success = _dataman_client1.clearSync((dm_item_t)item);
@ -433,7 +433,7 @@ DatamanTest::testAsyncReadInvalidIndex()
case State::Read: case State::Read:
state = State::ReadWait; state = State::ReadWait;
success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2);
if (!success) { if (!success) {
return false; return false;
@ -489,7 +489,7 @@ DatamanTest::testAsyncWriteInvalidIndex()
case State::Write: case State::Write:
state = State::WriteWait; state = State::WriteWait;
success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2);
if (!success) { if (!success) {
return false; return false;
@ -529,7 +529,7 @@ DatamanTest::testAsyncWriteInvalidIndex()
bool bool
DatamanTest::testAsyncReadBufferOverflow() DatamanTest::testAsyncReadBufferOverflow()
{ {
bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH); bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH);
return !success; return !success;
} }
@ -537,7 +537,8 @@ DatamanTest::testAsyncReadBufferOverflow()
bool bool
DatamanTest::testAsyncWriteBufferOverflow() DatamanTest::testAsyncWriteBufferOverflow()
{ {
bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, OVERFLOW_LENGTH); bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS_0, DM_KEY_SAFE_POINTS_MAX, _buffer_write,
OVERFLOW_LENGTH);
return !success; return !success;
} }
@ -715,7 +716,7 @@ DatamanTest::testAsyncWriteReadAllItemsMaxSize()
bool success = false; bool success = false;
State state = State::Write; State state = State::Write;
uint32_t item = DM_KEY_SAFE_POINTS; uint32_t item = DM_KEY_SAFE_POINTS_0;
uint32_t index = 0U; uint32_t index = 0U;
hrt_abstime start_time = hrt_absolute_time(); hrt_abstime start_time = hrt_absolute_time();
@ -828,7 +829,7 @@ DatamanTest::testAsyncClearAll()
State state = State::Clear; State state = State::Clear;
hrt_abstime start_time = hrt_absolute_time(); hrt_abstime start_time = hrt_absolute_time();
uint32_t item = DM_KEY_SAFE_POINTS; uint32_t item = DM_KEY_SAFE_POINTS_0;
//While loop represents a task //While loop represents a task
while (state != State::Exit) { while (state != State::Exit) {
@ -1098,19 +1099,19 @@ DatamanTest::testResetItems()
stats.num_items = 0; stats.num_items = 0;
stats.opaque_id = 0; stats.opaque_id = 0;
success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s)); sizeof(mission_stats_entry_s));
if (!success) { if (!success) {
PX4_ERR("failed to reset DM_KEY_FENCE_POINTS"); PX4_ERR("failed to reset DM_KEY_FENCE_POINTS_STATE");
return false; return false;
} }
success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s)); sizeof(mission_stats_entry_s));
if (!success) { if (!success) {
PX4_ERR("failed to reset DM_KEY_SAFE_POINTS"); PX4_ERR("failed to reset DM_KEY_SAFE_POINTS_STATE");
return false; return false;
} }