dataman: add dm_trylock & add lock for FENCE_POINTS items

src/modules/dataman/dataman.cpp

@@ -242,7 +242,8 @@ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
 
 /* Item type lock mutexes */
 static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS];
-static px4_sem_t g_sys_state_mutex;
+static px4_sem_t g_sys_state_mutex_mission;
+static px4_sem_t g_sys_state_mutex_fence;
 
 /* The data manager store file handle and file name */
 #if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
@@ -1154,22 +1155,48 @@ dm_clear(dm_item_t item)
 	return enqueue_work_item_and_wait_for_result(work);
 }
 
-/** Lock a data Item */
-__EXPORT void
+__EXPORT int
 dm_lock(dm_item_t item)
 {
 	/* Make sure data manager has been started and is not shutting down */
 	if (!is_running() || g_task_should_exit) {
-		return;
+		errno = EINVAL;
+		return -1;
 	}
 
 	if (item >= DM_KEY_NUM_KEYS) {
-		return;
+		errno = EINVAL;
+		return -1;
 	}
 
 	if (g_item_locks[item]) {
-		px4_sem_wait(g_item_locks[item]);
+		return px4_sem_wait(g_item_locks[item]);
 	}
+
+	errno = EINVAL;
+	return -1;
+}
+
+__EXPORT int
+dm_trylock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if (!is_running() || g_task_should_exit) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if (item >= DM_KEY_NUM_KEYS) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if (g_item_locks[item]) {
+		return px4_sem_trywait(g_item_locks[item]);
+	}
+
+	errno = EINVAL;
+	return -1;
 }
 
 /** Unlock a data Item */
@@ -1263,14 +1290,16 @@ task_main(int argc, char *argv[])
 		g_func_counts[i] = 0;
 	}
 
-	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
-	px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */
+	px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */
+	px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */
 
 	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
 		g_item_locks[i] = nullptr;
 	}
 
-	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
+	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission;
+	g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence;
 
 	g_task_should_exit = false;
 
@@ -1406,7 +1435,8 @@ end:
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	px4_sem_destroy(&g_work_queued_sema);
-	px4_sem_destroy(&g_sys_state_mutex);
+	px4_sem_destroy(&g_sys_state_mutex_mission);
+	px4_sem_destroy(&g_sys_state_mutex_fence);
 
 	return 0;
 }

src/modules/dataman/dataman.h

@@ -127,16 +127,30 @@ dm_write(
 	size_t buflen			/* Length in bytes of data to retrieve */
 );
 
-/** Lock all items of this type */
-__EXPORT void
+/**
+ * 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 clear */
+	dm_item_t item			/* The item type to lock */
 );
 
-/** Unlock all items of this type */
+/**
+ * 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 clear */
+	dm_item_t item			/* The item type to unlock */
 );
 
 /** Erase all items of this type */