dataman: add DatamanClient with sync functions

Rework of dataman

msg/CMakeLists.txt

@@ -60,6 +60,8 @@ set(msg_files
 	CollisionReport.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
+	DatamanRequest.msg
+	DatamanResponse.msg
 	DebugArray.msg
 	DebugKeyValue.msg
 	DebugValue.msg

msg/DatamanRequest.msg

@@ -0,0 +1,8 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 client_id
+uint8 request_type	# id/read/write/clear/lock/unlock
+uint8 item			# dm_item_t
+uint32 index
+uint8[56] data
+uint32 data_length

msg/DatamanResponse.msg

@@ -0,0 +1,18 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 client_id
+uint8 request_type	# id/read/write/clear/lock/unlock
+uint8 item			# dm_item_t
+uint32 index
+uint8[56] data
+
+uint8 STATUS_SUCCESS = 0
+uint8 STATUS_FAILURE_ID_ERR = 1
+uint8 STATUS_FAILURE_NO_DATA = 2
+uint8 STATUS_FAILURE_READ_FAILED = 3
+uint8 STATUS_FAILURE_WRITE_FAILED = 4
+uint8 STATUS_FAILURE_CLEAR_FAILED = 5
+uint8 STATUS_ALREADY_LOCKED = 6
+uint8 STATUS_ALREADY_UNLOCKED = 7
+uint8 STATUS_ITEM_LOCKED = 8
+uint8 status

src/lib/CMakeLists.txt

@@ -45,6 +45,7 @@ add_subdirectory(controllib EXCLUDE_FROM_ALL)
 add_subdirectory(conversion EXCLUDE_FROM_ALL)
 add_subdirectory(crc EXCLUDE_FROM_ALL)
 add_subdirectory(crypto EXCLUDE_FROM_ALL)
+add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
 add_subdirectory(drivers EXCLUDE_FROM_ALL)
 add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
 add_subdirectory(geo EXCLUDE_FROM_ALL)

src/lib/dataman_client/CMakeLists.txt

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(dataman_client DatamanClient.cpp)

src/lib/dataman_client/DatamanClient.cpp

@@ -0,0 +1,336 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file DatamanClient.cpp
+ */
+
+#include <dataman_client/DatamanClient.hpp>
+
+DatamanClient::DatamanClient()
+{
+	_dataman_request_pub.advertise();
+	_dataman_response_sub = orb_subscribe(ORB_ID(dataman_response));
+
+	if (_dataman_response_sub < 0) {
+		PX4_ERR("Failed to subscribe (%i)", errno);
+
+	} else {
+		// make sure we don't get any stale response by doing an orb_copy
+		dataman_response_s response{};
+		orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response);
+
+		_fds.fd = _dataman_response_sub;
+		_fds.events = POLLIN;
+
+		hrt_abstime timestamp = hrt_absolute_time();
+
+		dataman_request_s request = {
+			.timestamp = timestamp,
+			.request_type = DM_GET_ID
+		};
+
+		bool success = syncHandler(request, response, timestamp, 1000_ms);
+
+		if (success && (response.client_id > CLIENT_ID_NOT_SET)) {
+
+			_client_id = response.client_id;
+
+		} else {
+			PX4_ERR("Failed to get client ID!");
+		}
+	}
+}
+
+DatamanClient::~DatamanClient()
+{
+	if (_dataman_response_sub >= 0) {
+		orb_unsubscribe(_dataman_response_sub);
+	}
+}
+
+bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_response_s &response,
+				const hrt_abstime &start_time, hrt_abstime timeout)
+{
+	bool response_received = false;
+	int32_t ret = 0;
+	hrt_abstime time_elapsed = hrt_elapsed_time(&start_time);
+	_dataman_request_pub.publish(request);
+
+	while (!response_received && (time_elapsed < timeout)) {
+
+		uint32_t timeout_ms = 1;
+		ret = px4_poll(&_fds, 1, timeout_ms);
+
+		if (ret < 0) {
+			PX4_ERR("px4_poll returned error: %" PRIu32, ret);
+			break;
+
+		} else if (ret == 0) {
+
+			// No response received, send new request
+			_dataman_request_pub.publish(request);
+
+		} else {
+
+			bool updated = false;
+			orb_check(_dataman_response_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response);
+
+				if (response.client_id == request.client_id) {
+
+					if ((response.request_type == request.request_type) &&
+					    (response.item == request.item) &&
+					    (response.index == request.index)) {
+						response_received = true;
+						break;
+					}
+
+				} else if (request.client_id == CLIENT_ID_NOT_SET) {
+
+					// validate timestamp from response.data
+					if (0 == memcmp(&(request.timestamp), &(response.data), sizeof(hrt_abstime))) {
+						response_received = true;
+						break;
+					}
+				}
+			}
+		}
+
+		time_elapsed = hrt_elapsed_time(&start_time);
+	}
+
+	if (!response_received && ret >= 0) {
+		PX4_ERR("timeout after %" PRIu32 " ms!", static_cast<uint32_t>(timeout / 1000));
+	}
+
+	return response_received;
+}
+
+bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
+{
+	if (length > g_per_item_size[item]) {
+		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
+		return false;
+	}
+
+	bool success = false;
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	dataman_request_s request = {
+		.timestamp = timestamp,
+		.index = index,
+		.data_length = length,
+		.client_id = _client_id,
+		.request_type = DM_READ,
+		.item = item
+	};
+
+	dataman_response_s response{};
+	success = syncHandler(request, response, timestamp, timeout);
+
+	if (success) {
+
+		if (response.status != dataman_response_s::STATUS_SUCCESS) {
+
+			success = false;
+			PX4_ERR("readSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32,
+				response.status, static_cast<uint8_t>(item), index, length);
+
+		} else {
+			memcpy(buffer, response.data, length);
+		}
+	}
+
+	return success;
+}
+
+bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
+{
+	if (length > g_per_item_size[item]) {
+		PX4_ERR("Length  %" PRIu32 " can't fit in data size for item  %" PRIi8, length, static_cast<uint8_t>(item));
+		return false;
+	}
+
+	bool success = false;
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	dataman_request_s request = {
+		.timestamp = timestamp,
+		.index = index,
+		.data_length = length,
+		.client_id = _client_id,
+		.request_type = DM_WRITE,
+		.item = item
+	};
+
+	memcpy(request.data, buffer, length);
+
+	dataman_response_s response{};
+	success = syncHandler(request, response, timestamp, timeout);
+
+	if (success) {
+
+		if (response.status != dataman_response_s::STATUS_SUCCESS) {
+
+			success = false;
+			PX4_ERR("writeSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32,
+				response.status, static_cast<uint8_t>(item), index, length);
+		}
+	}
+
+	return success;
+}
+
+bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout)
+{
+	bool success = false;
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	dataman_request_s request = {
+		.timestamp = timestamp,
+		.client_id = _client_id,
+		.request_type = DM_CLEAR,
+		.item = item
+	};
+
+	dataman_response_s response{};
+	success = syncHandler(request, response, timestamp, timeout);
+
+	if (success) {
+
+		if (response.status != dataman_response_s::STATUS_SUCCESS) {
+
+			success = false;
+			PX4_ERR("clearSync failed! status=%" PRIu8 ", item=%" PRIu8,
+				response.status, static_cast<uint8_t>(item));
+		}
+	}
+
+	return success;
+}
+
+bool DatamanClient::lockSync(dm_item_t item, hrt_abstime timeout)
+{
+	bool success = true;
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	dataman_request_s request = {
+		.timestamp = timestamp,
+		.client_id = _client_id,
+		.request_type = DM_LOCK,
+		.item = item
+	};
+
+	dataman_response_s response{};
+	response.status = dataman_response_s::STATUS_ALREADY_LOCKED;
+
+	while (success && (response.status == dataman_response_s::STATUS_ALREADY_LOCKED)) {
+		success = syncHandler(request, response, timestamp, timeout);
+	}
+
+	if (success) {
+
+		if (response.status != dataman_response_s::STATUS_SUCCESS) {
+
+			success = false;
+
+			PX4_ERR("lockSync failed! status=%" PRIu8 ", item=%" PRIu8,
+				response.status, static_cast<uint8_t>(item));
+		}
+	}
+
+	return success;
+}
+
+bool DatamanClient::unlockSync(dm_item_t item, hrt_abstime timeout)
+{
+	bool success = false;
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	dataman_request_s request = {
+		.timestamp = timestamp,
+		.client_id = _client_id,
+		.request_type = DM_UNLOCK,
+		.item = item
+	};
+
+	dataman_response_s response{};
+	success = syncHandler(request, response, timestamp, timeout);
+
+	if (success) {
+
+		if (response.status != dataman_response_s::STATUS_SUCCESS) {
+
+			success = false;
+
+			if (response.status == dataman_response_s::STATUS_ALREADY_UNLOCKED) {
+				PX4_WARN("Dataman already unlocked for item=%" PRIu8,
+					 static_cast<uint8_t>(item));
+
+			} else {
+				PX4_ERR("unlockSync failed! status=%" PRIu8 ", item=%" PRIu8,
+					response.status, static_cast<uint8_t>(item));
+			}
+		}
+	}
+
+	return success;
+}
+
+bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer)
+{
+	bool success = false;
+	return success;
+}
+
+bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer)
+{
+	bool success = false;
+	return success;
+}
+
+void DatamanClient::update()
+{
+}
+
+bool DatamanClient::lastOperationCompleted(bool &success)
+{
+	return true;
+}
+
+DatamanCache::DatamanCache(int num_items)
+{
+}

src/lib/dataman_client/DatamanClient.hpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/dataman_request.h>
+#include <uORB/topics/dataman_response.h>
+#include <dataman/dataman.h>
+#include <lib/perf/perf_counter.h>
+
+using namespace time_literals;
+
+class DatamanClient
+{
+public:
+	DatamanClient();
+	~DatamanClient();
+
+	DatamanClient(const DatamanClient &) = delete;
+	DatamanClient &operator=(const DatamanClient &) = delete;
+
+	bool readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms);
+	bool writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms);
+	bool clearSync(dm_item_t item, hrt_abstime timeout = 1000_ms);
+	bool lockSync(dm_item_t item, hrt_abstime timeout = 1000_ms);
+	bool unlockSync(dm_item_t item, hrt_abstime timeout = 1000_ms);
+
+	bool readAsync(dm_item_t item, uint32_t index, uint8_t *buffer);
+	bool writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer);
+
+	void update();
+
+	bool lastOperationCompleted(bool &success);
+
+private:
+
+	enum class State {
+		Idle,
+		RequestSent
+	};
+
+	struct Request {
+		dm_item_t item;
+		unsigned index;
+		uint8_t *buffer;
+	};
+
+	/* Synchronous response/request handler */
+	bool syncHandler(const dataman_request_s &request, dataman_response_s &response,
+			 const hrt_abstime &start_time, hrt_abstime timeout);
+
+
+	//State _state{State::Idle};
+	//Request _active_request{};
+
+	int32_t _dataman_response_sub{};
+	uORB::Publication<dataman_request_s> _dataman_request_pub{ORB_ID(dataman_request)};
+
+	px4_pollfd_struct_t _fds;
+
+	uint8_t _client_id{0};
+
+	static constexpr uint8_t CLIENT_ID_NOT_SET{0};
+};
+
+
+class DatamanCache
+{
+public:
+	DatamanCache(int num_items);
+	~DatamanCache() = default;
+
+	void resize(int num_items);
+
+	void invalidate();
+
+	bool load(dm_item_t item, unsigned index);
+
+	bool loadWait(dm_item_t item, unsigned index, uint8_t *buffer, hrt_abstime timeout);
+
+	void update();
+
+	DatamanClient &client() { return _client; }
+
+private:
+	struct Item {
+		dataman_response_s data;
+		//State cache_state;
+	};
+	Item *_items{nullptr};
+
+	DatamanClient _client;
+};

src/modules/dataman/dataman.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,21 +42,29 @@
 #include <navigator/navigation.h>
 #include <uORB/topics/mission.h>
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 /** 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_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 */
+	DM_KEY_SAFE_POINTS = 0,			///< Safe points coordinates, safe point 0 is home point
+	DM_KEY_FENCE_POINTS,			///< Fence vertex coordinates
+	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
 	DM_KEY_COMPAT,
-	DM_KEY_NUM_KEYS			/* Total number of item types defined */
+	DM_KEY_NUM_KEYS					///< Total number of item types defined
 } dm_item_t;
 
+/** Types of function calls supported by the worker task */
+typedef enum {
+	DM_GET_ID = 0,		///< Get dataman client ID
+	DM_WRITE,			///< Write index for given item
+	DM_READ,			///< Read index for given item
+	DM_CLEAR,			///< Clear all index for given item
+	DM_LOCK,			///< Lock all items for given item types
+	DM_UNLOCK,			///< Unlock all items for given item types
+	DM_NUMBER_OF_FUNCS
+} dm_function_t;
+
+/** The maximum number of instances for each item type */
 #if defined(MEMORY_CONSTRAINED_SYSTEM)
 enum {
 	DM_KEY_SAFE_POINTS_MAX = 8,
@@ -67,7 +75,6 @@ enum {
 	DM_KEY_COMPAT_MAX = 1
 };
 #else
-/** The maximum number of instances for each item type */
 enum {
 	DM_KEY_SAFE_POINTS_MAX = 8,
 	DM_KEY_FENCE_POINTS_MAX = 64,
@@ -78,6 +85,32 @@ enum {
 };
 #endif
 
+/* 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_FENCE_POINTS_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
+	DM_KEY_MISSION_STATE_MAX,
+	DM_KEY_COMPAT_MAX
+};
+
+constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_safe_point_s);
+constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_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 mission_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_FENCE_POINT_SIZE,
+	MISSION_ITEM_SIZE,
+	MISSION_ITEM_SIZE,
+	MISSION_SIZE,
+	DATAMAN_COMPAT_SIZE
+};
+
 struct dataman_compat_s {
 	uint64_t key;
 };
@@ -89,57 +122,3 @@ struct dataman_compat_s {
 		       (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \
 		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \
 		       sizeof(struct dataman_compat_s))
-
-/** Retrieve from the data manager store */
-__EXPORT ssize_t
-dm_read(
-	dm_item_t item,			/* The item type to retrieve */
-	unsigned index,			/* The index of the item */
-	void *buffer,			/* Pointer to caller data buffer */
-	size_t buflen			/* Length in bytes of data to retrieve */
-);
-
-/** write to the data manager store */
-__EXPORT ssize_t
-dm_write(
-	dm_item_t  item,		/* The item type to store */
-	unsigned index,			/* The index of the item */
-	const void *buffer,		/* Pointer to caller data buffer */
-	size_t buflen			/* Length in bytes of data to retrieve */
-);
-
-/**
- * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
- * atomically).
- * Note that this lock is independent from dm_read & dm_write calls.
- * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
- */
-__EXPORT int
-dm_lock(
-	dm_item_t item			/* The item type to lock */
-);
-
-/**
- * Try to lock all items of a type (@see sem_trywait()).
- * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
- */
-__EXPORT int
-dm_trylock(
-	dm_item_t item			/* The item type to lock */
-);
-
-/** Unlock all items of a type */
-__EXPORT void
-dm_unlock(
-	dm_item_t item			/* The item type to unlock */
-);
-
-/** Erase all items of this type */
-__EXPORT int
-dm_clear(
-	dm_item_t item			/* The item type to clear */
-);
-
-#ifdef __cplusplus
-}
-#endif

src/modules/mavlink/CMakeLists.txt

@@ -131,6 +131,7 @@ px4_add_module(
 		adsb
 		airspeed
 		component_general_json # for checksums.h
+		dataman_client
 		drivers_accelerometer
 		drivers_gyroscope
 		drivers_magnetometer

src/modules/mavlink/mavlink_mission.cpp

@@ -84,27 +84,28 @@ MavlinkMissionManager::init_offboard_mission()
 		_dataman_init = true;
 
 		/* lock MISSION_STATE item */
-		int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+		bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
 
-		if (dm_lock_ret != 0) {
+		if (!success_lock) {
 			PX4_ERR("DM_KEY_MISSION_STATE lock failed");
 		}
 
 		mission_s mission_state;
-		int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+		bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
+							sizeof(mission_s));
 
 		/* unlock MISSION_STATE item */
-		if (dm_lock_ret == 0) {
-			dm_unlock(DM_KEY_MISSION_STATE);
+		if (success_lock) {
+			_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
 		}
 
-		if (ret > 0) {
+		if (success) {
 			_dataman_id = (dm_item_t)mission_state.dataman_id;
 			_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
 			_current_seq = mission_state.current_seq;
 
-		} else if (ret < 0) {
-			PX4_WARN("offboard mission init failed (%i)", ret);
+		} else {
+			PX4_WARN("offboard mission init failed");
 		}
 
 		load_geofence_stats();
@@ -115,33 +116,35 @@ MavlinkMissionManager::init_offboard_mission()
 	_my_dataman_id = _dataman_id;
 }
 
-int
+bool
 MavlinkMissionManager::load_geofence_stats()
 {
 	mission_stats_entry_s stats;
 	// initialize fence points count
-	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+						sizeof(mission_stats_entry_s));
 
-	if (ret == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
 		_geofence_update_counter = stats.update_counter;
 	}
 
-	return ret;
+	return success;
 }
 
-int
+bool
 MavlinkMissionManager::load_safepoint_stats()
 {
 	mission_stats_entry_s stats;
 	// initialize safe points count
-	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+						sizeof(mission_stats_entry_s));
 
-	if (ret == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
 	}
 
-	return ret;
+	return success;
 }
 
 /**
@@ -160,20 +163,21 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
 	/* update mission state in dataman */
 
 	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+	bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
 
-	if (dm_lock_ret != 0) {
+	if (!success_lock) {
 		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
 	}
 
-	int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+	bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),
+			sizeof(mission_s));
 
 	/* unlock MISSION_STATE item */
-	if (dm_lock_ret == 0) {
-		dm_unlock(DM_KEY_MISSION_STATE);
+	if (success_lock) {
+		_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
 	}
 
-	if (res == sizeof(mission_s)) {
+	if (success) {
 		/* update active mission state */
 		_dataman_id = dataman_id;
 		_count[MAV_MISSION_TYPE_MISSION] = count;
@@ -206,9 +210,10 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
 	stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
 
 	/* update stats in dataman */
-	int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+			sizeof(mission_stats_entry_s));
 
-	if (res == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		_count[MAV_MISSION_TYPE_FENCE] = count;
 
 	} else {
@@ -233,9 +238,10 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
 	stats.update_counter = ++_safepoint_update_counter;
 
 	/* update stats in dataman */
-	int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+			sizeof(mission_stats_entry_s));
 
-	if (res == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		_count[MAV_MISSION_TYPE_RALLY] = count;
 
 	} else {
@@ -298,21 +304,20 @@ void
 MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 	mission_item_s mission_item{};
-	int16_t bytes_read = 0;
 	bool read_success = false;
 
 	switch (_mission_type) {
 
 	case MAV_MISSION_TYPE_MISSION: {
-			bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s));
-			read_success = (bytes_read == sizeof(mission_item_s));
+			read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_item),
+								sizeof(mission_item_s));
 		}
 		break;
 
 	case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
 			mission_fence_point_s mission_fence_point;
-			bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s));
-			read_success = (bytes_read == sizeof(mission_fence_point_s));
+			read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1,
+								reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
 
 			mission_item.nav_cmd = mission_fence_point.nav_cmd;
 			mission_item.frame = mission_fence_point.frame;
@@ -332,8 +337,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 
 	case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
 			mission_safe_point_s mission_safe_point;
-			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
-			read_success = (bytes_read == sizeof(mission_safe_point_s));
+			read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast<uint8_t *>(&mission_safe_point),
+								sizeof(mission_safe_point_s));
 
 			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
 			mission_item.frame = mission_safe_point.frame;
@@ -385,13 +390,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 
 		if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
 			mavlink_log_critical(_mavlink->get_mavlink_log_pub(),
-					     "Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t",
-					     (uint8_t)_mission_type, bytes_read);
+					     "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type);
 			/* EVENT
-			 * @description Mission type: {1}. Number of bytes read: {2}
+			 * @description Mission type: {1}
 			 */
-			events::send<uint8_t, int16_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
-						       "Mission: Unable to read from storage", _mission_type, bytes_read);
+			events::send<uint8_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
+					      "Mission: Unable to read from storage", _mission_type);
 		}
 
 		PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id);
@@ -961,9 +965,9 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 				// switching back to idle
 				PX4_DEBUG("locking fence dataman items");
 
-				int ret = dm_lock(DM_KEY_FENCE_POINTS);
+				bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS);
 
-				if (ret == 0) {
+				if (success) {
 					_geofence_locked = true;
 
 				} else {
@@ -1009,10 +1013,10 @@ MavlinkMissionManager::switch_to_idle_state()
 	// when switching to idle, we *always* check if the lock was held and release it.
 	// This is to ensure we don't end up in a state where we forget to release it.
 	if (_geofence_locked) {
-		dm_unlock(DM_KEY_FENCE_POINTS);
-		_geofence_locked = false;
-
-		PX4_DEBUG("unlocking geofence");
+		if (_dataman_client.unlockSync(DM_KEY_FENCE_POINTS)) {
+			_geofence_locked = false;
+			PX4_DEBUG("unlocking geofence");
+		}
 	}
 
 	_state = MAVLINK_WPM_STATE_IDLE;
@@ -1131,7 +1135,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 				} else {
 					dm_item_t dm_item = _transfer_dataman_id;
 
-					write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s);
+					write_failed = !_dataman_client.writeSync(dm_item, wp.seq, reinterpret_cast<uint8_t *>(&mission_item),
+							sizeof(struct mission_item_s));
 
 					if (!write_failed) {
 						/* waypoint marked as current */
@@ -1167,8 +1172,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 				mission_fence_point.frame = mission_item.frame;
 
 				if (!check_failed) {
-					write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point,
-								sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s);
+					write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1,
+							reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
 				}
 
 			}
@@ -1180,8 +1185,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 				mission_safe_point.lon = mission_item.lon;
 				mission_safe_point.alt = mission_item.altitude;
 				mission_safe_point.frame = mission_item.frame;
-				write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point,
-							sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
+				write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1,
+						reinterpret_cast<uint8_t *>(&mission_safe_point), sizeof(mission_safe_point_s));
 			}
 			break;
 

src/modules/mavlink/mavlink_mission.h

@@ -45,7 +45,7 @@
 
 #pragma once
 
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/mission_result.h>
@@ -95,6 +95,8 @@ private:
 	enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE};	///< Current state
 	enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION};	///< mission type of current transmission (only one at a time possible)
 
+	DatamanClient _dataman_client{};
+
 	uint64_t		_time_last_recv{0};
 	uint64_t		_time_last_sent{0};
 
@@ -132,7 +134,7 @@ private:
 
 	static uint16_t		_geofence_update_counter;
 	static uint16_t		_safepoint_update_counter;
-	bool			_geofence_locked{false};		///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
+	bool			_geofence_locked{false};		///< if true, we currently hold the dataman lock for the geofence (transaction in progress)
 
 	MavlinkRateLimiter	_slow_rate_limiter{100 * 1000};		///< Rate limit sending of the current WP sequence to 10 Hz
 
@@ -168,10 +170,10 @@ private:
 	int update_safepoint_count(unsigned count);
 
 	/** load geofence stats from dataman */
-	int load_geofence_stats();
+	bool load_geofence_stats();
 
 	/** load safe point stats from dataman */
-	int load_safepoint_stats();
+	bool load_safepoint_stats();
 
 	/**
 	 *  @brief Sends an waypoint ack message

src/modules/navigator/CMakeLists.txt

@@ -51,6 +51,7 @@ px4_add_module(
 		geofence.cpp
 		vtol_takeoff.cpp
 	DEPENDS
+		dataman_client
 		geo
 		adsb
 		geofence_breach_avoidance

src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp

@@ -34,7 +34,6 @@
 #include <gtest/gtest.h>
 #include "geofence_breach_avoidance.h"
 #include "fake_geofence.hpp"
-#include "dataman_mocks.hpp"
 #include <parameters/param.h>
 
 using namespace matrix;

src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp

@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file dataman_mocks.h
- * Provides a minimal dataman implementation to compile against for testing
- *
- * @author Roman Bapst
- * @author Julian Kent
- */
-#pragma once
-
-#include <dataman/dataman.h>
-extern "C" {
-	__EXPORT ssize_t
-	dm_read(
-		dm_item_t item,			/* The item type to retrieve */
-		unsigned index,			/* The index of the item */
-		void *buffer,			/* Pointer to caller data buffer */
-		size_t buflen			/* Length in bytes of data to retrieve */
-	) {return 0;};
-
-	/** write to the data manager store */
-	__EXPORT ssize_t
-	dm_write(
-		dm_item_t  item,		/* The item type to store */
-		unsigned index,			/* The index of the item */
-		const void *buffer,		/* Pointer to caller data buffer */
-		size_t buflen			/* Length in bytes of data to retrieve */
-	) {return 0;};
-
-	/**
-	 * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
-	 * atomically).
-	 * Note that this lock is independent from dm_read & dm_write calls.
-	 * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
-	 */
-	__EXPORT int
-	dm_lock(
-		dm_item_t item			/* The item type to lock */
-	) {return 0;};
-
-	/**
-	 * Try to lock all items of a type (@see sem_trywait()).
-	 * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
-	 */
-	__EXPORT int
-	dm_trylock(
-		dm_item_t item			/* The item type to lock */
-	) {return 0;};
-
-	/** Unlock all items of a type */
-	__EXPORT void
-	dm_unlock(
-		dm_item_t item			/* The item type to unlock */
-	) {};
-
-	/** Erase all items of this type */
-	__EXPORT int
-	dm_clear(
-		dm_item_t item			/* The item type to clear */
-	) {return 0;};
-}
-

src/modules/navigator/geofence.cpp

@@ -43,7 +43,7 @@
 
 #include <ctype.h>
 
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <systemlib/mavlink_log.h>
@@ -75,23 +75,26 @@ 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
-	if (dm_lock(DM_KEY_FENCE_POINTS) != 0) {
+	bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS);
+
+	if (!success) {
 		PX4_ERR("lock failed");
 		return;
 	}
 
 	_updateFence();
-	dm_unlock(DM_KEY_FENCE_POINTS);
+	_dataman_client.unlockSync(DM_KEY_FENCE_POINTS);
 }
 
 void Geofence::_updateFence()
 {
 	// initialize fence points count
 	mission_stats_entry_s stats;
-	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	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 (ret == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		num_fence_items = stats.num_items;
 		_update_counter = stats.update_counter;
 	}
@@ -104,8 +107,10 @@ void Geofence::_updateFence()
 		mission_fence_point_s mission_fence_point;
 		bool is_circle_area = false;
 
-		if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) !=
-		    sizeof(mission_fence_point_s)) {
+		success = _dataman_client.readSync(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");
 			break;
 		}
@@ -306,22 +311,25 @@ bool Geofence::isBelowMaxAltitude(float altitude)
 
 bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 {
-	// the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means
+	// 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
-	if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) {
+	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;
-	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+					   sizeof(mission_stats_entry_s));
 
-	if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) {
+	if (success && _update_counter != stats.update_counter) {
 		_updateFence();
 	}
 
 	if (isEmpty()) {
-		dm_unlock(DM_KEY_FENCE_POINTS);
+		_dataman_client.unlockSync(DM_KEY_FENCE_POINTS);
 		/* Empty fence -> accept all points */
 		return true;
 	}
@@ -329,7 +337,7 @@ 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) {
-			dm_unlock(DM_KEY_FENCE_POINTS);
+			_dataman_client.unlockSync(DM_KEY_FENCE_POINTS);
 			return false;
 		}
 	}
@@ -375,7 +383,7 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 		}
 	}
 
-	dm_unlock(DM_KEY_FENCE_POINTS);
+	_dataman_client.unlockSync(DM_KEY_FENCE_POINTS);
 
 	return (!had_inclusion_areas || inside_inclusion) && outside_exclusion;
 }
@@ -394,13 +402,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
 	bool c = false;
 
 	for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
-		if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i,
-			    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
+
+		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));
+
+		if (!success) {
 			break;
 		}
 
-		if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j,
-			    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
+		success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index + j,
+						   reinterpret_cast<uint8_t *>(&temp_vertex_j), sizeof(mission_fence_point_s));
+
+		if (!success) {
 			break;
 		}
 
@@ -426,9 +439,10 @@ 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));
 
-	if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point,
-		    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
+	if (!success) {
 		PX4_ERR("dm_read failed");
 		return false;
 	}
@@ -531,7 +545,10 @@ Geofence::loadFromFile(const char *filename)
 				}
 			}
 
-			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, &vertex, sizeof(vertex)) != sizeof(vertex)) {
+			bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast<uint8_t *>(&vertex),
+					sizeof(vertex));
+
+			if (!success) {
 				goto error;
 			}
 
@@ -555,22 +572,31 @@ Geofence::loadFromFile(const char *filename)
 	if (gotVertical && pointCounter > 2) {
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
 		events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
-		ret_val = PX4_OK;
+		ret_val = PX4_ERROR;
 
 		/* do a second pass, now that we know the number of vertices */
 		for (int seq = 1; seq <= pointCounter; ++seq) {
 			mission_fence_point_s mission_fence_point;
 
-			if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) ==
-			    sizeof(mission_fence_point_s)) {
+			bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
+								sizeof(mission_fence_point_s));
+
+			if (success) {
 				mission_fence_point.vertex_count = pointCounter;
-				dm_write(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s));
+				_dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
+							  sizeof(mission_fence_point_s));
 			}
 		}
 
 		mission_stats_entry_s stats;
 		stats.num_items = pointCounter;
-		ret_val = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+
+		bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+				sizeof(mission_stats_entry_s));
+
+		if (success) {
+			ret_val = PX4_OK;
+		}
 
 	} else {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t");
@@ -586,7 +612,7 @@ error:
 
 int Geofence::clearDm()
 {
-	dm_clear(DM_KEY_FENCE_POINTS);
+	_dataman_client.clearSync(DM_KEY_FENCE_POINTS);
 	updateFence();
 	return PX4_OK;
 }

src/modules/navigator/geofence.h

@@ -42,6 +42,7 @@
 
 #include <float.h>
 
+#include <dataman_client/DatamanClient.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <px4_platform_common/module_params.h>
 #include <drivers/drv_hrt.h>
@@ -166,6 +167,8 @@ private:
 	Navigator   *_navigator{nullptr};
 	PolygonInfo *_polygons{nullptr};
 
+	DatamanClient _dataman_client{};
+
 	hrt_abstime _last_horizontal_range_warning{0};
 	hrt_abstime _last_vertical_range_warning{0};
 

src/modules/navigator/mission.cpp

@@ -50,7 +50,7 @@
 
 #include <string.h>
 #include <drivers/drv_hrt.h>
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <lib/geo/geo.h>
@@ -75,7 +75,7 @@ void Mission::mission_init()
 	// init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start
 	mission_s mission{};
 
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+	if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s))) {
 		if ((mission.timestamp != 0)
 		    && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) {
 			if (mission.count > 0) {
@@ -88,7 +88,7 @@ void Mission::mission_init()
 			// initialize mission state in dataman
 			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
 			mission.timestamp = hrt_absolute_time();
-			dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+			_dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
 		}
 	}
 }
@@ -124,14 +124,11 @@ Mission::on_inactive()
 		/* load missions from storage */
 		mission_s mission_state = {};
 
-		dm_lock(DM_KEY_MISSION_STATE);
-
 		/* read current state */
-		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+		bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
+							sizeof(mission_s));
 
-		dm_unlock(DM_KEY_MISSION_STATE);
-
-		if (read_res == sizeof(mission_s)) {
+		if (success) {
 			_mission.dataman_id = mission_state.dataman_id;
 			_mission.count = mission_state.count;
 			_current_mission_index = mission_state.current_seq;
@@ -464,10 +461,12 @@ Mission::find_mission_land_start()
 	bool found_land_start_marker = false;
 
 	for (size_t i = 1; i < _mission.count; i++) {
-		const ssize_t len = sizeof(missionitem);
 		missionitem_prev = missionitem; // store the last mission item before reading a new one
 
-		if (dm_read(dm_current, i, &missionitem, len) != len) {
+		bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
+							sizeof(mission_item_s));
+
+		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			PX4_ERR("dataman read failure");
 			break;
@@ -668,9 +667,11 @@ Mission::advance_mission()
 
 				for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
 					struct mission_item_s missionitem = {};
-					const ssize_t len = sizeof(missionitem);
 
-					if (dm_read(dm_current, i, &missionitem, len) != len) {
+					bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
+										sizeof(mission_item_s));
+
+					if (!success) {
 						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 						PX4_ERR("dataman read failure");
 						break;
@@ -1641,13 +1642,14 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 			return false;
 		}
 
-		const ssize_t len = sizeof(struct mission_item_s);
-
 		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
 		struct mission_item_s mission_item_tmp;
 
 		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
+		bool success = _dataman_client.readSync(dm_item, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
+							sizeof(mission_item_s));
+
+		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t");
 			events::send<uint16_t>(events::ID("mission_failed_to_read_wp"), events::Log::Error,
@@ -1668,7 +1670,10 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 					(mission_item_tmp.do_jump_current_count)++;
 
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
+					success = _dataman_client.writeSync(dm_item, *mission_index_ptr, reinterpret_cast<uint8_t *>(&mission_item_tmp),
+									    sizeof(struct mission_item_s));
+
+					if (!success) {
 						/* not supposed to happen unless the datamanager can't access the dataman */
 						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t");
 						events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error,
@@ -1718,16 +1723,17 @@ Mission::save_mission_state()
 	mission_s mission_state = {};
 
 	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+	bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
 
-	if (dm_lock_ret != 0) {
+	if (!success_lock) {
 		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
 	}
 
 	/* read current state */
-	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+	bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
+						sizeof(mission_s));
 
-	if (read_res == sizeof(mission_s)) {
+	if (success) {
 		/* data read successfully, check dataman ID and items count */
 		if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) {
 			/* navigator may modify only sequence, write modified state only if it changed */
@@ -1735,7 +1741,10 @@ Mission::save_mission_state()
 				mission_state.current_seq = _current_mission_index;
 				mission_state.timestamp = hrt_absolute_time();
 
-				if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+				success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
+								    sizeof(mission_s));
+
+				if (!success) {
 
 					PX4_ERR("Can't save mission state");
 				}
@@ -1756,15 +1765,18 @@ Mission::save_mission_state()
 		events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state");
 
 		/* write modified state only if changed */
-		if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+		success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
+						    sizeof(mission_s));
+
+		if (!success) {
 
 			PX4_ERR("Can't save mission state");
 		}
 	}
 
 	/* unlock MISSION_STATE item */
-	if (dm_lock_ret == 0) {
-		dm_unlock(DM_KEY_MISSION_STATE);
+	if (success_lock) {
+		_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
 	}
 }
 
@@ -1821,9 +1833,9 @@ Mission::check_mission_valid(bool force)
 void
 Mission::reset_mission(struct mission_s &mission)
 {
-	dm_lock(DM_KEY_MISSION_STATE);
+	_dataman_client.lockSync(DM_KEY_MISSION_STATE);
 
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+	if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s))) {
 		if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
 			/* set current item to 0 */
 			mission.current_seq = 0;
@@ -1836,7 +1848,9 @@ Mission::reset_mission(struct mission_s &mission)
 					struct mission_item_s item;
 					const ssize_t len = sizeof(struct mission_item_s);
 
-					if (dm_read(dm_current, index, &item, len) != len) {
+					bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast<uint8_t *>(&item), sizeof(mission_item_s));
+
+					if (!success) {
 						PX4_WARN("could not read mission item during reset");
 						break;
 					}
@@ -1844,7 +1858,9 @@ Mission::reset_mission(struct mission_s &mission)
 					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
 						item.do_jump_current_count = 0;
 
-						if (dm_write(dm_current, index, &item, len) != len) {
+						success = _dataman_client.writeSync(dm_current, index, reinterpret_cast<uint8_t *>(&item), len);
+
+						if (!success) {
 							PX4_WARN("could not save mission item during reset");
 							break;
 						}
@@ -1863,10 +1879,10 @@ Mission::reset_mission(struct mission_s &mission)
 			mission.current_seq = 0;
 		}
 
-		dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+		_dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
 	}
 
-	dm_unlock(DM_KEY_MISSION_STATE);
+	_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
 }
 
 bool
@@ -1883,7 +1899,7 @@ Mission::need_to_reset_mission()
 }
 
 int32_t
-Mission::index_closest_mission_item() const
+Mission::index_closest_mission_item()
 {
 	int32_t min_dist_index(0);
 	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
@@ -1892,9 +1908,11 @@ Mission::index_closest_mission_item() const
 
 	for (size_t i = 0; i < _mission.count; i++) {
 		struct mission_item_s missionitem = {};
-		const ssize_t len = sizeof(missionitem);
 
-		if (dm_read(dm_current, i, &missionitem, len) != len) {
+		bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&missionitem),
+							sizeof(mission_item_s));
+
+		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			PX4_ERR("dataman read failure");
 			break;

src/modules/navigator/mission.h

@@ -52,7 +52,7 @@
 
 #include <float.h>
 
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
@@ -234,7 +234,7 @@ private:
 	/**
 	 * Return the index of the closest mission item to the current global position.
 	 */
-	int32_t index_closest_mission_item() const;
+	int32_t index_closest_mission_item();
 
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
@@ -323,6 +323,7 @@ private:
 
 	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
 	mission_s		_mission {};
+	DatamanClient	_dataman_client{};
 
 	int32_t _current_mission_index{-1};
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -78,9 +78,11 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission)
 
 	for (size_t i = 0; i < mission.count; i++) {
 		struct mission_item_s missionitem = {};
-		const ssize_t len = sizeof(struct mission_item_s);
 
-		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+		bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast<uint8_t *>(&missionitem),
+							sizeof(mission_item_s));
+
+		if (!success) {
 			_navigator->get_mission_result()->warning = true;
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return false;
@@ -116,9 +118,11 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al
 	if (_navigator->get_geofence().valid()) {
 		for (size_t i = 0; i < mission.count; i++) {
 			struct mission_item_s missionitem = {};
-			const ssize_t len = sizeof(missionitem);
 
-			if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast<uint8_t *>(&missionitem),
+								sizeof(mission_item_s));
+
+			if (!success) {
 				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 				return false;
 			}

src/modules/navigator/mission_feasibility_checker.h

@@ -42,7 +42,7 @@
 
 #pragma once
 
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <uORB/topics/mission.h>
 #include <px4_platform_common/module_params.h>
 #include "MissionFeasibility/FeasibilityChecker.hpp"
@@ -75,4 +75,6 @@ public:
 	 * Returns true if mission is feasible and false otherwise
 	 */
 	bool checkMissionFeasible(const mission_s &mission);
+
+	DatamanClient _dataman_client{};
 };

src/modules/navigator/navigator_main.cpp

@@ -48,7 +48,7 @@
 #include <float.h>
 #include <sys/stat.h>
 
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/adsb/AdsbConflict.h>

src/modules/navigator/rtl.cpp

@@ -42,7 +42,7 @@
 
 #include "rtl.h"
 #include "navigator.h"
-#include <dataman/dataman.h>
+#include <dataman_client/DatamanClient.hpp>
 #include <px4_platform_common/events.h>
 
 #include <lib/geo/geo.h>
@@ -181,10 +181,11 @@ void RTL::find_RTL_destination()
 	// compare to safe landing positions
 	mission_safe_point_s closest_safe_point {};
 	mission_stats_entry_s stats;
-	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+						sizeof(mission_stats_entry_s));
 	int num_safe_points = 0;
 
-	if (ret == sizeof(mission_stats_entry_s)) {
+	if (success) {
 		num_safe_points = stats.num_items;
 	}
 
@@ -194,8 +195,10 @@ void RTL::find_RTL_destination()
 	for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
 		mission_safe_point_s mission_safe_point;
 
-		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) !=
-		    sizeof(mission_safe_point_s)) {
+		success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, current_seq, reinterpret_cast<uint8_t *>(&mission_safe_point),
+						   sizeof(mission_safe_point_s));
+
+		if (!success) {
 			PX4_ERR("dm_read failed");
 			continue;
 		}

src/modules/navigator/rtl.h

@@ -53,6 +53,7 @@
 #include <uORB/topics/wind.h>
 #include <matrix/math.hpp>
 #include <lib/geo/geo.h>
+#include <dataman_client/DatamanClient.hpp>
 
 class Navigator;
 
@@ -156,6 +157,8 @@ private:
 		}
 	};
 
+	DatamanClient _dataman_client{};
+
 	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
 
 	hrt_abstime _destination_check_time{0};

src/systemcmds/tests/CMakeLists.txt

@@ -36,7 +36,7 @@ set(srcs
 	test_bezierQuad.cpp
 	test_bitset.cpp
 	test_bson.cpp
-	test_dataman.c
+	test_dataman.cpp
 	test_file.c
 	test_file2.c
 	test_float.cpp
@@ -93,6 +93,7 @@ px4_add_module(
 	SRCS
 		${srcs}
 	DEPENDS
+		dataman_client
 		version
 	)
 

src/systemcmds/tests/test_dataman.c

@@ -1,220 +0,0 @@
-/****************************************************************************
- *
- *  Copyright (C) 2018-2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file test_dataman.c
- * Tests for the data manager.
- */
-
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/posix.h>
-
-#include <sys/types.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-
-#include <drivers/drv_board_led.h>
-#include <drivers/drv_hrt.h>
-#include <semaphore.h>
-
-#include "tests_main.h"
-
-#include "dataman/dataman.h"
-
-static px4_sem_t *sems;
-static bool *task_returned_error;
-int test_dataman(int argc, char *argv[]);
-
-#define NUM_MISSIONS_TEST 50
-
-#define DM_MAX_DATA_SIZE sizeof(struct mission_s)
-
-static int
-task_main(int argc, char *argv[])
-{
-	char buffer[DM_MAX_DATA_SIZE];
-
-	PX4_INFO("Starting dataman test task %s", argv[2]);
-	/* try to read an invalid item */
-	int my_id = atoi(argv[2]);
-
-	/* try to read an invalid item */
-	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
-		PX4_ERR("%d read an invalid item failed", my_id);
-		goto fail;
-	}
-
-	/* try to read an invalid index */
-	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
-		PX4_ERR("%d read an invalid index failed", my_id);
-		goto fail;
-	}
-
-	srand(hrt_absolute_time() ^ my_id);
-	unsigned hit = 0;
-	unsigned miss = 0;
-	hrt_abstime wstart = hrt_absolute_time();
-
-	for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) {
-		memset(buffer, my_id, sizeof(buffer));
-		buffer[1] = i;
-		unsigned hash = i ^ my_id;
-		unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
-
-		int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, len);
-		//PX4_INFO("ret: %d", ret);
-
-		if (ret != len) {
-			PX4_WARN("task %d: write failed ret=%d, index: %d, length: %d", my_id, ret, hash, len);
-			goto fail;
-		}
-
-		if (i % (NUM_MISSIONS_TEST / 10) == 0) {
-			PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST);
-		}
-
-		px4_usleep(rand() & ((64 * 1024) - 1));
-	}
-
-	hrt_abstime rstart = hrt_absolute_time();
-	hrt_abstime wend = rstart;
-
-	for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) {
-		unsigned hash = i ^ my_id;
-		ssize_t len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer));
-		ssize_t len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
-
-		if (len2 != len) {
-			PX4_WARN("task %d: read failed length test, index %d, ret=%zd, len=%zd", my_id, hash, len2, len);
-			goto fail;
-		}
-
-		if (buffer[0] == my_id) {
-			hit++;
-
-			if (len2 != len) {
-				PX4_WARN("task %d: read failed length test, index %d, wanted %zd, got %zd", my_id, hash, len, len2);
-				goto fail;
-			}
-
-			if (buffer[1] != i) {
-				PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
-				goto fail;
-			}
-
-		} else {
-			miss++;
-		}
-	}
-
-	hrt_abstime rend = hrt_absolute_time();
-	PX4_INFO("task %d pass, hit %d, miss %d, io time read %" PRIu64 "ms. write %" PRIu64 "ms.",
-		 my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000);
-	px4_sem_post(sems + my_id);
-	return 0;
-
-fail:
-	PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x",
-		my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
-	px4_sem_post(sems + my_id);
-	task_returned_error[my_id] = true;
-	return -1;
-}
-
-int test_dataman(int argc, char *argv[])
-{
-	int i = 0;
-	unsigned num_tasks = 4;
-	char buffer[DM_MAX_DATA_SIZE];
-
-	if (argc > 1) {
-		num_tasks = atoi(argv[1]);
-	}
-
-	sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
-	task_returned_error = (bool *)calloc(num_tasks, sizeof(bool));
-	PX4_INFO("Running %d tasks", num_tasks);
-
-	for (i = 0; i < num_tasks; i++) {
-		int task;
-
-		char a[16];
-		snprintf(a, 16, "%d", i);
-
-		char *av[] = {"tests_dataman", a, NULL};
-
-		px4_sem_init(sems + i, 1, 0);
-		/* sems use case is a signal */
-		px4_sem_setprotocol(sems + i, SEM_PRIO_NONE);
-
-		/* start the task */
-		if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, task_main, av)) <= 0) {
-			PX4_ERR("task start failed");
-		}
-	}
-
-	for (i = 0; i < num_tasks; i++) {
-		px4_sem_wait(sems + i);
-		px4_sem_destroy(sems + i);
-	}
-
-	free(sems);
-
-	bool got_error = false;
-
-	for (i = 0; i < num_tasks; i++) {
-		if (task_returned_error[i]) {
-			got_error = true;
-			break;
-		}
-	}
-
-	free(task_returned_error);
-
-	if (got_error) {
-		return -1;
-	}
-
-	for (i = 0; i < NUM_MISSIONS_TEST; i++) {
-		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
-			break;
-		}
-	}
-
-	return 0;
-}

src/systemcmds/tests/test_dataman.cpp

@@ -0,0 +1,433 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_dataman.cpp
+ * Tests for Dataman.
+ */
+
+#include <unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <px4_platform_common/px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <pthread.h>
+
+#include "dataman_client/DatamanClient.hpp"
+
+class DatamanTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+	bool testSyncReadInvalidItem();
+	bool testSyncWriteInvalidItem();
+
+	bool testSyncReadInvalidIndex();
+	bool testSyncWriteInvalidIndex();
+
+	bool testSyncReadBufferOverflow();
+	bool testSyncWriteBufferOverflow();
+
+	bool testMutipleClients();
+
+	bool testSyncWriteReadAllItemsMaxSize();
+
+	bool testSyncClearAll();
+
+	bool testResetItems();
+
+	static void *testLockThread(void *arg);
+
+	DatamanClient _dataman_client1{};
+	DatamanClient _dataman_client2{};
+	DatamanClient _dataman_client3{};
+	DatamanClient _dataman_client_thread1{};
+
+	static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_ITEM_SIZE};
+	static_assert(sizeof(dataman_response_s::data) == DM_MAX_DATA_SIZE, "data size != DM_MAX_DATA_SIZE");
+
+	uint8_t _buffer_read[DM_MAX_DATA_SIZE];
+	uint8_t _buffer_write[DM_MAX_DATA_SIZE];
+
+	static constexpr uint32_t OVERFLOW_LENGTH = sizeof(_buffer_write) + 1;
+};
+
+bool
+DatamanTest::testSyncReadInvalidItem()
+{
+
+	bool success = _dataman_client1.readSync(DM_KEY_NUM_KEYS, 0, _buffer_read, sizeof(_buffer_read));
+	return !success;
+}
+
+bool
+DatamanTest::testSyncWriteInvalidItem()
+{
+	bool success = _dataman_client1.writeSync(DM_KEY_NUM_KEYS, 0, _buffer_write, sizeof(_buffer_write));
+	return !success;
+}
+
+
+bool
+DatamanTest::testSyncReadInvalidIndex()
+{
+	bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 0);
+	return !success;
+}
+
+bool
+DatamanTest::testSyncWriteInvalidIndex()
+{
+	bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 0);
+	return !success;
+}
+
+bool
+DatamanTest::testSyncReadBufferOverflow()
+{
+	bool success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_read, OVERFLOW_LENGTH);
+	return !success;
+}
+
+bool
+DatamanTest::testSyncWriteBufferOverflow()
+{
+	bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_write, OVERFLOW_LENGTH);
+	return !success;
+}
+
+bool
+DatamanTest::testMutipleClients()
+{
+	bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_write, sizeof(_buffer_write));
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_read, sizeof(_buffer_read));
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client2.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_write, sizeof(_buffer_write));
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client3.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_read, sizeof(_buffer_read));
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_write, sizeof(_buffer_write));
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_read, sizeof(_buffer_read));
+
+	if (!success) {
+		return false;
+	}
+
+	//Test locking
+	success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE);
+
+	if (!success) {
+		return false;
+	}
+
+	//Check if already locked
+	success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE, 10_ms);
+
+	if (success) {
+		return false;
+	}
+
+	//Check if already locked from another client
+	success = _dataman_client2.lockSync(DM_KEY_MISSION_STATE, 10_ms);
+
+	if (success) {
+		return false;
+	}
+
+	//Check if can write while locked
+	success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, _buffer_write, g_per_item_size[DM_KEY_MISSION_STATE]);
+
+	if (!success) {
+		return false;
+	}
+
+	//Check if can read  while locked
+	success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, _buffer_read, g_per_item_size[DM_KEY_MISSION_STATE]);
+
+	if (!success) {
+		return false;
+	}
+
+	//Compare content from buffers
+	for (uint32_t i = 0; i < g_per_item_size[DM_KEY_MISSION_STATE]; ++i) {
+		if (_buffer_read[i] != _buffer_write[i]) {
+			return false;
+		}
+	}
+
+	//Check if can write while locked from another client
+	success = _dataman_client2.writeSync(DM_KEY_MISSION_STATE, 0, _buffer_write, g_per_item_size[DM_KEY_MISSION_STATE]);
+
+	if (success) {
+		return false;
+	}
+
+	//Check if can read  while locked from another client
+	success = _dataman_client2.readSync(DM_KEY_MISSION_STATE, 0, _buffer_read, g_per_item_size[DM_KEY_MISSION_STATE]);
+
+	if (success) {
+		return false;
+	}
+
+	//Check if can unlock while locked from another client
+	success = _dataman_client2.unlockSync(DM_KEY_MISSION_STATE);
+
+	if (success) {
+		return false;
+	}
+
+	//Check if can unlock
+	success = _dataman_client1.unlockSync(DM_KEY_MISSION_STATE);
+
+	if (!success) {
+		return false;
+	}
+
+	//Lock from another thread and to test retry lock mechanism
+	pthread_t thread{};
+	uint32_t ret = pthread_create(&thread, NULL, &testLockThread, this);
+
+	if (ret != 0) {
+		printf("pthread_create failed: %" PRIu32 "\n", ret);
+		return false;
+	}
+
+	px4_usleep(50_ms);
+
+	//Should fail since timeout is to short
+	success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE, 10_ms);
+
+	if (success) {
+		return false;
+	}
+
+	//Should be able to lock since the task in the thread should unlock the item in the meantime
+	success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE);
+
+	if (!success) {
+		return false;
+	}
+
+	success = _dataman_client1.unlockSync(DM_KEY_MISSION_STATE);
+
+	if (!success) {
+		return false;
+	}
+
+	pthread_join(thread, nullptr);
+
+	return success;
+}
+
+void *DatamanTest::testLockThread(void *arg)
+{
+	DatamanTest *dataman_test = (DatamanTest *)arg;
+	dataman_test->_dataman_client_thread1.lockSync(DM_KEY_MISSION_STATE);
+	px4_usleep(200_ms);
+	dataman_test->_dataman_client_thread1.unlockSync(DM_KEY_MISSION_STATE);
+	px4_usleep(200_ms);
+
+	return nullptr;
+}
+
+bool
+DatamanTest::testSyncWriteReadAllItemsMaxSize()
+{
+	bool success = false;
+
+	// Iterate all items
+	for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) {
+
+		// writeSync
+		for (uint32_t index = 0U; index < g_per_item_max_index[item]; ++index) {
+
+			// Prepare write buffer
+			for (uint32_t i = 0; i < g_per_item_size[item]; ++i) {
+				_buffer_write[i] = (uint8_t)(index % UINT8_MAX);
+			}
+
+			success = _dataman_client1.writeSync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]);
+
+			if (!success) {
+				PX4_ERR("writeSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index);
+				return false;
+			}
+		}
+
+		// readSync
+		for (volatile uint32_t index = 0U; index < g_per_item_max_index[item]; ++index) {
+
+			success = _dataman_client1.readSync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]);
+
+			if (!success) {
+				PX4_ERR("readSync failed at item =  %" PRIu32 ", index = %" PRIu32, item, index);
+				return false;
+			}
+
+			// Check read buffer
+			for (volatile uint32_t i = 0U; i < g_per_item_size[item]; ++i) {
+
+				uint8_t expected_value = (index % UINT8_MAX);
+
+				if (expected_value != _buffer_read[i]) {
+					PX4_ERR("readSync failed at item = %" PRIu32 ", index =  %" PRIu32 ", element= %" PRIu32 ", expected:  %" PRIu8
+						", received:  %" PRIu8,
+						item, index, i, expected_value, _buffer_read[i]);
+					return false;
+				}
+			}
+		}
+	}
+
+	return true;;
+}
+
+bool
+DatamanTest::testSyncClearAll()
+{
+	bool success = false;
+
+	// Iterate all items
+	for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) {
+
+		success = _dataman_client1.clearSync((dm_item_t)item);
+
+		if (!success) {
+			PX4_ERR("clearSync failed at item =  %" PRIu32, item);
+			return false;
+		}
+	}
+
+	return success;
+}
+
+bool
+DatamanTest::testResetItems()
+{
+	bool success = false;
+
+	mission_s mission{};
+	mission.timestamp = hrt_absolute_time();
+	mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	mission.count = 0;
+	mission.current_seq = 0;
+
+	success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
+
+	if (!success) {
+		PX4_ERR("failed to reset DM_KEY_MISSION_STATE");
+		return false;
+	}
+
+	success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
+
+	if (!success) {
+		PX4_ERR("failed to read DM_KEY_MISSION_STATE");
+		return false;
+	}
+
+	mission_stats_entry_s stats;
+	stats.num_items = 0;
+	stats.update_counter = 1;
+
+	success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+					     sizeof(mission_stats_entry_s));
+
+	if (!success) {
+		PX4_ERR("failed to reset DM_KEY_FENCE_POINTS");
+		return false;
+	}
+
+	success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
+					     sizeof(mission_stats_entry_s));
+
+	if (!success) {
+		PX4_ERR("failed to reset DM_KEY_SAFE_POINTS");
+		return false;
+	}
+
+	return success;
+}
+
+bool DatamanTest::run_tests()
+{
+	ut_run_test(testSyncReadInvalidItem);
+	ut_run_test(testSyncWriteInvalidItem);
+
+	ut_run_test(testSyncReadInvalidIndex);
+	ut_run_test(testSyncWriteInvalidIndex);
+
+	ut_run_test(testSyncReadBufferOverflow);
+	ut_run_test(testSyncWriteBufferOverflow);
+
+	ut_run_test(testMutipleClients);
+
+	ut_run_test(testSyncWriteReadAllItemsMaxSize);
+
+	ut_run_test(testSyncClearAll);
+
+	ut_run_test(testResetItems);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_dataman, DatamanTest)