forked from Archive/PX4-Autopilot
dataman: extend for double storage geofence and safe points
This commit is contained in:
Konrad
Daniel Agar
parent
dfa56d474a
commit
50f1abaef1
|
|
@ -129,8 +129,12 @@ static unsigned g_func_counts[DM_NUMBER_OF_FUNCS];
|
|||
|
||||
/* 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] = {
|
||||
g_per_item_size[DM_KEY_SAFE_POINTS] + DM_SECTOR_HDR_SIZE,
|
||||
g_per_item_size[DM_KEY_FENCE_POINTS] + DM_SECTOR_HDR_SIZE,
|
||||
g_per_item_size[DM_KEY_SAFE_POINTS_0] + 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_1] + 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -582,8 +586,8 @@ _file_initialize(unsigned max_offset)
|
|||
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_FENCE_POINTS, 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_FENCE_POINTS_STATE, 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;
|
||||
|
|
@ -877,10 +881,6 @@ Each type has a specific type and a fixed maximum amount of storage items, so th
|
|||
### Implementation
|
||||
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");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("dataman", "system");
|
||||
|
|
|
|||
|
|
@ -44,8 +44,12 @@
|
|||
|
||||
/** Types of items that the data manager can store */
|
||||
typedef enum {
|
||||
DM_KEY_SAFE_POINTS = 0, ///< Safe points coordinates, safe point 0 is home point
|
||||
DM_KEY_FENCE_POINTS, ///< Fence vertex coordinates
|
||||
DM_KEY_SAFE_POINTS_0 = 0, ///< Safe points storage 0
|
||||
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_1, ///< (alternate between 0 and 1)
|
||||
DM_KEY_MISSION_STATE, ///< Persistent mission state
|
||||
|
|
@ -66,7 +70,9 @@ typedef enum {
|
|||
#if defined(MEMORY_CONSTRAINED_SYSTEM)
|
||||
enum {
|
||||
DM_KEY_SAFE_POINTS_MAX = 8,
|
||||
DM_KEY_SAFE_POINTS_STATE_MAX = 1,
|
||||
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_1_MAX = NUM_MISSIONS_SUPPORTED,
|
||||
DM_KEY_MISSION_STATE_MAX = 1,
|
||||
|
|
@ -75,7 +81,9 @@ enum {
|
|||
#else
|
||||
enum {
|
||||
DM_KEY_SAFE_POINTS_MAX = 32,
|
||||
DM_KEY_SAFE_POINTS_STATE_MAX = 1,
|
||||
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_1_MAX = NUM_MISSIONS_SUPPORTED,
|
||||
DM_KEY_MISSION_STATE_MAX = 1,
|
||||
|
|
@ -86,7 +94,11 @@ enum {
|
|||
/* table of maximum number of instances for each item type */
|
||||
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_STATE_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_1_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_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_STATE_SIZE = sizeof(struct mission_stats_entry_s);
|
||||
constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s);
|
||||
constexpr uint32_t MISSION_SIZE = sizeof(struct mission_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 */
|
||||
static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
|
||||
MISSION_SAFE_POINT_SIZE,
|
||||
MISSION_SAFE_POINT_SIZE,
|
||||
MISSION_SAFE_POINT_STATE_SIZE,
|
||||
MISSION_FENCE_POINT_SIZE,
|
||||
MISSION_FENCE_POINT_SIZE,
|
||||
MISSION_FENCE_POINT_STATE_SIZE,
|
||||
MISSION_ITEM_SIZE,
|
||||
MISSION_ITEM_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 */
|
||||
#define DM_COMPAT_VERSION 4ULL
|
||||
#define DM_COMPAT_VERSION 5ULL
|
||||
|
||||
#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) + \
|
||||
|
|
|
|||
|
|
@ -110,7 +110,7 @@ MavlinkMissionManager::load_geofence_stats()
|
|||
{
|
||||
mission_stats_entry_s stats;
|
||||
// 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));
|
||||
|
||||
if (success) {
|
||||
|
|
@ -126,7 +126,7 @@ MavlinkMissionManager::load_safepoint_stats()
|
|||
{
|
||||
mission_stats_entry_s stats;
|
||||
// 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));
|
||||
|
||||
if (success) {
|
||||
|
|
@ -182,7 +182,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
|
|||
stats.opaque_id = crc32;
|
||||
|
||||
/* 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));
|
||||
|
||||
if (success) {
|
||||
|
|
@ -214,7 +214,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
|
|||
stats.opaque_id = crc32;
|
||||
|
||||
/* 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));
|
||||
|
||||
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
|
||||
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));
|
||||
|
||||
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;
|
||||
|
||||
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));
|
||||
}
|
||||
break;
|
||||
|
|
@ -1152,7 +1152,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
mission_fence_point.frame = mission_item.frame;
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
|
|
@ -1160,7 +1160,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
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);
|
||||
}
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -148,8 +148,8 @@ private:
|
|||
2; ///< Error count limit before stopping to report FS errors
|
||||
static constexpr uint16_t MAX_COUNT[] = {
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
|
||||
DM_KEY_FENCE_POINTS_MAX - 1,
|
||||
DM_KEY_SAFE_POINTS_MAX - 1
|
||||
DM_KEY_FENCE_POINTS_MAX,
|
||||
DM_KEY_SAFE_POINTS_MAX
|
||||
}; /**< Maximum number of mission items for each type
|
||||
(fence & safe points use the first item for the stats) */
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
||||
|
|
|
|||
|
|
@ -153,14 +153,14 @@ DatamanTest::testSyncWriteInvalidItem()
|
|||
bool
|
||||
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;
|
||||
}
|
||||
|
||||
bool
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -238,7 +238,7 @@ DatamanTest::testSyncWriteReadAllItemsMaxSize()
|
|||
bool success = false;
|
||||
|
||||
// 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
|
||||
for (uint32_t index = 0U; index < _max_index[item]; ++index) {
|
||||
|
|
@ -289,7 +289,7 @@ DatamanTest::testSyncClearAll()
|
|||
bool success = false;
|
||||
|
||||
// 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);
|
||||
|
||||
|
|
@ -433,7 +433,7 @@ DatamanTest::testAsyncReadInvalidIndex()
|
|||
case State::Read:
|
||||
|
||||
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) {
|
||||
return false;
|
||||
|
|
@ -489,7 +489,7 @@ DatamanTest::testAsyncWriteInvalidIndex()
|
|||
case State::Write:
|
||||
|
||||
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) {
|
||||
return false;
|
||||
|
|
@ -529,7 +529,7 @@ DatamanTest::testAsyncWriteInvalidIndex()
|
|||
bool
|
||||
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;
|
||||
}
|
||||
|
|
@ -537,7 +537,8 @@ DatamanTest::testAsyncReadBufferOverflow()
|
|||
bool
|
||||
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;
|
||||
}
|
||||
|
|
@ -715,7 +716,7 @@ DatamanTest::testAsyncWriteReadAllItemsMaxSize()
|
|||
bool success = false;
|
||||
State state = State::Write;
|
||||
|
||||
uint32_t item = DM_KEY_SAFE_POINTS;
|
||||
uint32_t item = DM_KEY_SAFE_POINTS_0;
|
||||
uint32_t index = 0U;
|
||||
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
|
|
@ -828,7 +829,7 @@ DatamanTest::testAsyncClearAll()
|
|||
|
||||
State state = State::Clear;
|
||||
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 (state != State::Exit) {
|
||||
|
|
@ -1098,19 +1099,19 @@ DatamanTest::testResetItems()
|
|||
stats.num_items = 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));
|
||||
|
||||
if (!success) {
|
||||
PX4_ERR("failed to reset DM_KEY_FENCE_POINTS");
|
||||
PX4_ERR("failed to reset DM_KEY_FENCE_POINTS_STATE");
|
||||
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));
|
||||
|
||||
if (!success) {
|
||||
PX4_ERR("failed to reset DM_KEY_SAFE_POINTS");
|
||||
PX4_ERR("failed to reset DM_KEY_SAFE_POINTS_STATE");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue