From 9fd137e88e9d2e43ae6f602569f39ecf23a29b5e Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 5 Dec 2023 13:35:03 +0100 Subject: [PATCH] mavlink_mission: add alternating storage for geofence and safe points on upload This way the old points are kept on an upload error. --- msg/Mission.msg | 2 + src/modules/mavlink/mavlink_mission.cpp | 107 ++++++++++++++++++------ src/modules/mavlink/mavlink_mission.h | 10 ++- src/modules/navigator/geofence.cpp | 38 +++++---- src/modules/navigator/geofence.h | 2 - src/modules/navigator/navigation.h | 3 +- src/modules/navigator/rtl.cpp | 6 +- 7 files changed, 118 insertions(+), 50 deletions(-) diff --git a/msg/Mission.msg b/msg/Mission.msg index 01573380f8..a923193da8 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -1,5 +1,7 @@ uint64 timestamp # time since system start (microseconds) uint8 mission_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 fence_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 safepoint_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 8cd0f79944..c33151e660 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,6 +58,8 @@ using matrix::wrap_2pi; dm_item_t MavlinkMissionManager::_mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; +dm_item_t MavlinkMissionManager::_safepoint_dataman_id = DM_KEY_SAFE_POINTS_0; +dm_item_t MavlinkMissionManager::_fence_dataman_id = DM_KEY_FENCE_POINTS_0; bool MavlinkMissionManager::_dataman_init = false; uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 }; @@ -91,6 +93,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : } _my_mission_dataman_id = _mission_dataman_id; + _my_fence_dataman_id = _fence_dataman_id; + _my_safepoint_dataman_id = _safepoint_dataman_id; } void @@ -116,6 +120,8 @@ MavlinkMissionManager::load_geofence_stats() if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _crc32[MAV_MISSION_TYPE_FENCE] = stats.opaque_id; + _fence_dataman_id = static_cast(stats.dataman_id); + _my_fence_dataman_id = _fence_dataman_id; } return success; @@ -132,6 +138,8 @@ MavlinkMissionManager::load_safepoint_stats() if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; _crc32[MAV_MISSION_TYPE_RALLY] = stats.opaque_id; + _safepoint_dataman_id = static_cast(stats.dataman_id); + _my_safepoint_dataman_id = _safepoint_dataman_id; } return success; @@ -146,14 +154,16 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1 { /* update active mission state */ _mission_dataman_id = mission_dataman_id; + _my_mission_dataman_id = _mission_dataman_id; _count[MAV_MISSION_TYPE_MISSION] = count; _crc32[MAV_MISSION_TYPE_MISSION] = crc32; _current_seq = seq; - _my_mission_dataman_id = _mission_dataman_id; mission_s mission{}; mission.timestamp = hrt_absolute_time(); mission.mission_dataman_id = mission_dataman_id; + mission.fence_dataman_id = _fence_dataman_id; + mission.safepoint_dataman_id = _safepoint_dataman_id; mission.count = count; mission.current_seq = seq; mission.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; @@ -175,11 +185,15 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1 } int -MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32) +MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32) { + _fence_dataman_id = fence_dataman_id; + _my_fence_dataman_id = fence_dataman_id; + mission_stats_entry_s stats; stats.num_items = count; stats.opaque_id = crc32; + stats.dataman_id = fence_dataman_id; /* update stats in dataman */ bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stats), @@ -207,11 +221,15 @@ MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32) } int -MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32) +MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32) { + _safepoint_dataman_id = safepoint_dataman_id; + _my_safepoint_dataman_id = safepoint_dataman_id; + mission_stats_entry_s stats; stats.num_items = count; stats.opaque_id = crc32; + stats.dataman_id = safepoint_dataman_id; /* update stats in dataman */ bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast(&stats), @@ -303,7 +321,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_STATE, seq, + read_success = _dataman_client.readSync(_fence_dataman_id, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); mission_item.nav_cmd = mission_fence_point.nav_cmd; @@ -323,7 +341,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_0, seq, reinterpret_cast(&mission_item), + read_success = _dataman_client.readSync(_safepoint_dataman_id, seq, reinterpret_cast(&mission_item), sizeof(mission_item_s)); } break; @@ -917,11 +935,12 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_FENCE: - update_geofence_count(0, 0); + update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, 0, 0); break; case MAV_MISSION_TYPE_RALLY: - update_safepoint_count(0, 0); + update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : DM_KEY_SAFE_POINTS_0, 0, + 0); break; default: @@ -936,13 +955,34 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + _transfer_dataman_id = (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + break; + + case MAV_MISSION_TYPE_FENCE: + _transfer_dataman_id = (_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : + DM_KEY_FENCE_POINTS_0); // use inactive storage for transmission + break; + + case MAV_MISSION_TYPE_RALLY: + _transfer_dataman_id = (_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0); // use inactive storage for transmission + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + _transfer_in_progress = false; + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_INVALID); + return; + } + _state = MAVLINK_WPM_STATE_GETLIST; _transfer_seq = 0; _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; - _transfer_mission_dataman_id = (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission _transfer_current_seq = -1; _transfer_land_start_marker = -1; _transfer_land_marker = -1; @@ -1099,7 +1139,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { - write_failed = !_dataman_client.writeSync(_transfer_mission_dataman_id, wp.seq, + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), sizeof(struct mission_item_s)); @@ -1142,7 +1182,6 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (mission_item.vertex_count < 3) { // feasibility check PX4_ERR("Fence: too few vertices"); check_failed = true; - update_geofence_count(0, 0); } } else { @@ -1152,7 +1191,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_STATE, wp.seq, + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } @@ -1160,7 +1199,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_0, wp.seq, + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), sizeof(mission_item_s), 2_s); } break; @@ -1173,7 +1212,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } if (write_failed || check_failed) { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_mission_dataman_id); + PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1208,15 +1247,30 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) case MAV_MISSION_TYPE_MISSION: _land_start_marker = _transfer_land_start_marker; _land_marker = _transfer_land_marker; - update_active_mission(_transfer_mission_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) { + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + } + break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(_transfer_count, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) { + ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(_transfer_count, _transfer_current_crc32); + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) { + ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + break; default: @@ -1269,11 +1323,13 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_FENCE: - ret = update_geofence_count(0, 0); + ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, + 0, 0); break; case MAV_MISSION_TYPE_RALLY: - ret = update_safepoint_count(0, 0); + ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0, 0, 0); break; case MAV_MISSION_TYPE_ALL: @@ -1281,8 +1337,10 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) _land_marker = -1; update_active_mission(_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0); - ret = update_geofence_count(0, 0); - ret = update_safepoint_count(0, 0) || ret; + ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, + 0, 0); + ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : + DM_KEY_SAFE_POINTS_0, 0, 0) || ret; break; default: @@ -1785,11 +1843,13 @@ void MavlinkMissionManager::check_active_mission() if (_mission_sub.updated()) { _mission_sub.update(); - if (_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) { + if ((_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) + || (_my_fence_dataman_id != (dm_item_t) _mission_sub.get().fence_dataman_id)) { load_geofence_stats(); } - if (_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) { + if ((_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) + || (_my_safepoint_dataman_id != (dm_item_t) _mission_sub.get().safepoint_dataman_id)) { load_safepoint_stats(); } @@ -1797,7 +1857,6 @@ void MavlinkMissionManager::check_active_mission() || (_my_mission_dataman_id != (dm_item_t)_mission_sub.get().mission_dataman_id)) { PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); init_offboard_mission(_mission_sub.get()); - _my_mission_dataman_id = _mission_dataman_id; send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], MAV_MISSION_TYPE_MISSION, _crc32[MAV_MISSION_TYPE_MISSION]); } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 75a4eb52b3..2251ef9e61 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -107,7 +107,11 @@ private: unsigned _filesystem_errcount{0}; ///< File system error count static dm_item_t _mission_dataman_id; ///< Global Dataman storage ID for active mission + static dm_item_t _safepoint_dataman_id; ///< Global dataman storage id for active safepoints + static dm_item_t _fence_dataman_id; ///< Global dataman storage id for active geofence dm_item_t _my_mission_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID for mission + dm_item_t _my_safepoint_dataman_id{DM_KEY_SAFE_POINTS_0}; ///< class Dataman storage ID for safepoints + dm_item_t _my_fence_dataman_id{DM_KEY_FENCE_POINTS_0}; ///< class Dataman storage ID for geofence static bool _dataman_init; ///< Dataman initialized @@ -117,7 +121,7 @@ private: int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) - dm_item_t _transfer_mission_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current mission transmission + dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission uint16_t _transfer_count{0}; ///< Items count in current transmission uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission @@ -172,10 +176,10 @@ private: bool write_to_dataman = true); /** store the geofence count to dataman */ - int update_geofence_count(unsigned count, uint32_t crc32); + int update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32); /** store the safepoint count to dataman */ - int update_safepoint_count(unsigned count, uint32_t crc32); + int update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32); /** load geofence stats from dataman */ bool load_geofence_stats(); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 73a18e0c09..4f48d768a5 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -140,7 +140,7 @@ void Geofence::run() } for (int index = 0; index < _dataman_cache.size(); ++index) { - _dataman_cache.load(DM_KEY_FENCE_POINTS_0, index); + _dataman_cache.load(static_cast(_stats.dataman_id), index); } _dataman_state = DatamanState::Load; @@ -191,7 +191,7 @@ void Geofence::_updateFence() while (current_seq < _dataman_cache.size()) { - bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, current_seq, + bool success = _dataman_cache.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); @@ -412,14 +412,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_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + i, + dm_item_t fence_dataman_id{static_cast(_stats.dataman_id)}; + bool success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + i, reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); if (!success) { break; } - success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + j, + success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + j, reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); if (!success) { @@ -448,7 +449,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_0, polygon.dataman_index, + bool success = _dataman_cache.loadWait(static_cast(_stats.dataman_id), polygon.dataman_index, reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); if (!success) { @@ -491,8 +492,18 @@ Geofence::loadFromFile(const char *filename) const char commentChar = '#'; int ret_val = PX4_ERROR; - /* Make sure no data is left in the datamanager */ - clearDm(); + mission_stats_entry_s stat; + { + const bool success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast(&stat), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("Could not read fence dataman state"); + return PX4_ERROR; + } + } + + dm_item_t write_fence_dataman_id{static_cast(stat.dataman_id) == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0}; /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); @@ -554,7 +565,7 @@ Geofence::loadFromFile(const char *filename) } } - bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, pointCounter, reinterpret_cast(&vertex), + bool success = _dataman_client.writeSync(write_fence_dataman_id, pointCounter, reinterpret_cast(&vertex), sizeof(vertex)); if (!success) { @@ -588,13 +599,13 @@ Geofence::loadFromFile(const char *filename) for (int seq = 0; seq < pointCounter; ++seq) { mission_fence_point_s mission_fence_point; - bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_0, seq, reinterpret_cast(&mission_fence_point), + bool success = _dataman_client.readSync(write_fence_dataman_id, seq, reinterpret_cast(&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_0, seq, reinterpret_cast(&mission_fence_point), + _dataman_client.writeSync(write_fence_dataman_id, seq, reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } } @@ -622,13 +633,6 @@ error: return ret_val; } -int Geofence::clearDm() -{ - _dataman_client.clearSync(DM_KEY_FENCE_POINTS_STATE); - updateFence(); - return PX4_OK; -} - bool Geofence::isHomeRequired() { bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 3f2ecbba61..f2f057dc9a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -108,8 +108,6 @@ public: virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); - int clearDm(); - bool valid(); /** diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 02de89f1fe..77d24f46dd 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -205,7 +205,8 @@ struct mission_item_s { struct mission_stats_entry_s { uint32_t opaque_id; /**< opaque identifier for current stored mission stats */ uint16_t num_items; /**< total number of items stored (excluding this one) */ - uint8_t padding[2]; + uint8_t dataman_id; /**< dm_item_t storage place*/ + uint8_t padding[1]; }; /** diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 51354a4e79..043a03721f 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -111,7 +111,7 @@ void RTL::updateDatamanCache() } for (int index = 0; index < _dataman_cache_safepoint.size(); ++index) { - _dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS_0, index); + _dataman_cache_safepoint.load(static_cast(_stats.dataman_id), index); } _dataman_state = DatamanState::Load; @@ -397,7 +397,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit 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_0, current_seq, + bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_safe_point), sizeof(mission_item_s), 500_ms); @@ -629,7 +629,7 @@ land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) 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_0, current_seq, + bool success_mission_item = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, reinterpret_cast(&mission_item), sizeof(mission_item_s));